Skip to content

Commit fea1f3b

Browse files
v1x2v1x2
committed
light it up (not done)
1 parent f7ba57b commit fea1f3b

File tree

2 files changed

+156
-0
lines changed

2 files changed

+156
-0
lines changed

‎src/main/java/frc/team5115/RobotContainer.java‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
import edu.wpi.first.math.geometry.Rotation2d;
77
import edu.wpi.first.math.geometry.Translation2d;
88
import edu.wpi.first.networktables.GenericEntry;
9+
import edu.wpi.first.wpilibj.XboxController;
910
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
1011
import edu.wpi.first.wpilibj2.command.Command;
1112
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
@@ -38,6 +39,7 @@
3839
import frc.team5115.subsystems.intake.IntakeIO;
3940
import frc.team5115.subsystems.intake.IntakeIOSim;
4041
import frc.team5115.subsystems.intake.IntakeIOSparkMax;
42+
import frc.team5115.subsystems.lights.Lights;
4143
import frc.team5115.subsystems.shooter.Shooter;
4244
import frc.team5115.subsystems.shooter.ShooterIO;
4345
import frc.team5115.subsystems.shooter.ShooterIOSim;
@@ -62,6 +64,7 @@ public class RobotContainer {
6264
private final Feeder feeder;
6365
private final Shooter shooter;
6466
private final Climber climber;
67+
private final Lights lights;
6568
// test
6669
// Controller
6770
private final CommandXboxController joyDrive = new CommandXboxController(0);
@@ -92,6 +95,9 @@ public RobotContainer() {
9295
new ModuleIOSparkMax(2),
9396
new ModuleIOSparkMax(3));
9497
vision = new PhotonVision(drivetrain);
98+
lights = new Lights(0, 20);
99+
lights.start();
100+
95101
// vision = null;
96102
noteDetectedEntry =
97103
Shuffleboard.getTab("SmartDashboard").add("Has note?", false).getEntry();
@@ -111,6 +117,7 @@ public RobotContainer() {
111117
gyro, new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim());
112118
vision = null;
113119
noteDetectedEntry = null;
120+
lights = null;
114121
break;
115122

116123
default:
@@ -127,6 +134,7 @@ public RobotContainer() {
127134
gyro, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {});
128135
vision = null;
129136
noteDetectedEntry = null;
137+
lights = null;
130138
break;
131139
}
132140

@@ -223,6 +231,12 @@ public void robotPeriodic() {
223231
if (noteDetectedEntry != null) {
224232
noteDetectedEntry.setBoolean(feeder.noteDetected());
225233
}
234+
//
235+
boolean aligning = joyDrive.getHID().getRawButton(XboxController.Button.kB.value);
236+
boolean inRange = false; // ?
237+
if(lights != null){
238+
lights.update(aligning, inRange);
239+
}
226240
}
227241

228242
/**

‎src/main/java/frc/team5115/subsystems/lights/Lights.java‎

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
package frc.team5115.subsystems.lights;
2+
import java.util.function.Function;
3+
4+
import edu.wpi.first.wpilibj.AddressableLED;
5+
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
6+
import edu.wpi.first.wpilibj.DigitalInput;
7+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
8+
import frc.team5115.Constants;
9+
10+
public class Lights extends SubsystemBase {
11+
private enum AnimationState { Idle, ControllingNote, Aligning, InRange }
12+
13+
private final int period = 2;
14+
private final double tailLength = 5;
15+
private final double decay = 1d / tailLength;
16+
private final int minPower = 10;
17+
private final int maxPower = 250;
18+
19+
private final AddressableLED leds;
20+
private final AddressableLEDBuffer buffer;
21+
public final int ledCount;
22+
23+
private int timer_idle;
24+
private int counter_idle;
25+
private int direction_idle;
26+
27+
private int timer_align;
28+
private int position_align;
29+
30+
private DigitalInput reflectiveSensor;
31+
32+
public Lights(int port, int ledCount){
33+
this.ledCount = ledCount;
34+
leds = new AddressableLED(port);
35+
buffer = new AddressableLEDBuffer(ledCount);
36+
reflectiveSensor = new DigitalInput(Constants.SHOOTER_SENSOR_ID);
37+
38+
}
39+
40+
public void start(){
41+
leds.start();
42+
}
43+
44+
public void stop(){
45+
leds.stop();
46+
}
47+
48+
public void update(boolean aligning, boolean inRange) {
49+
AnimationState state = AnimationState.Idle;
50+
51+
if (inRange) {
52+
state = AnimationState.InRange;
53+
} else if (aligning) {
54+
state = AnimationState.Aligning;
55+
} else if (!reflectiveSensor.get()) {
56+
state = AnimationState.ControllingNote;
57+
}
58+
59+
switch (state) {
60+
case ControllingNote:
61+
setUniformColor(0, 150, 0);
62+
break;
63+
case Aligning:
64+
updateAlignAnimation();
65+
break;
66+
case InRange:
67+
timer_align = 0;
68+
position_align = 0;
69+
setUniformColor(0xA5, 0x10, 0x90);
70+
break;
71+
case Idle:
72+
default:
73+
updateIdleAnimation();
74+
break;
75+
}
76+
}
77+
private void updateAlignAnimation() {
78+
timer_align ++;
79+
if (timer_align >= period) {
80+
timer_align = 0;
81+
} else {
82+
return;
83+
}
84+
85+
position_align++;
86+
if (position_align == ledCount / 2) {
87+
position_align = 0;
88+
}
89+
iterateAllLeds((index) -> {
90+
final boolean powered =
91+
index == position_align || // the current position
92+
index == ledCount-1 - position_align || // the opposite side
93+
index == position_align - 1 || // the left of the current position
94+
index == ledCount-1 - position_align + 1; // the right of the opposite side
95+
96+
if (powered) {
97+
return new Integer[] { 0xA5, 0x10, 0x90 };
98+
} else {
99+
return new Integer[] { 0, 0, 0 };
100+
}
101+
});
102+
}
103+
104+
public void updateIdleAnimation() {
105+
timer_idle ++;
106+
if (timer_idle >= period) {
107+
timer_idle = 0;
108+
} else {
109+
return;
110+
}
111+
112+
counter_idle += direction_idle;
113+
if (counter_idle == ledCount || counter_idle == -1) {
114+
direction_idle = -direction_idle;
115+
counter_idle += 2 * direction_idle;
116+
}
117+
iterateAllLeds((index) -> {
118+
double percent = buffer.getLED(index).red - minPower / 1d / maxPower - decay;
119+
percent = Math.max(percent, 0);
120+
121+
if (index == counter_idle) {
122+
percent = 1.0;
123+
}
124+
125+
final double power = (percent * (maxPower - minPower)) + minPower;
126+
return new Integer[] { (int)power, 0, 0 };
127+
});
128+
}
129+
public void setUniformColor(int r, int g, int b){
130+
iterateAllLeds((index) -> {
131+
return new Integer[] {r, g, b};
132+
});
133+
}
134+
public void iterateAllLeds(Function<Integer, Integer[]> function){
135+
for(int i = 0; i <ledCount; i++){
136+
Integer[] color = function.apply(i);
137+
buffer.setRGB(i, color[0], color[1], color[2]);
138+
}
139+
leds.setData(buffer);
140+
}
141+
142+
}

0 commit comments

Comments
 (0)