|
2 | 2 |
|
3 | 3 | import static edu.wpi.first.units.Units.Seconds; |
4 | 4 |
|
| 5 | +import edu.wpi.first.wpilibj.LEDPattern; |
| 6 | +import edu.wpi.first.wpilibj.util.Color; |
| 7 | +import frc.robot.constants.LEDConstants; |
5 | 8 | import java.util.Map; |
6 | 9 | import java.util.Set; |
7 | | - |
8 | 10 | import org.strykeforce.telemetry.measurable.MeasurableSubsystem; |
9 | 11 | import org.strykeforce.telemetry.measurable.Measure; |
10 | 12 |
|
11 | | -import edu.wpi.first.wpilibj.LEDPattern; |
12 | | -import edu.wpi.first.wpilibj.util.Color; |
13 | | -import frc.robot.constants.LEDConstants; |
| 13 | +public class LEDSubsystem extends MeasurableSubsystem { |
| 14 | + private LEDIO io; |
| 15 | + private LEDPattern base = LEDPattern.solid(Color.kBlack); |
| 16 | + private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); |
| 17 | + private LEDPattern algea = |
| 18 | + LEDPattern.steps( |
| 19 | + Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength / 3, Color.kBlack)); |
14 | 20 |
|
15 | | -public class LEDSubsystem extends MeasurableSubsystem{ |
16 | | - private LEDIO io; |
17 | | - private LEDPattern base = LEDPattern.solid(Color.kBlack); |
18 | | - private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); |
19 | | - private LEDPattern algea = LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength/3, Color.kBlack)); |
| 21 | + private LEDPattern currentLimiting = |
| 22 | + LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(1), Seconds.of(2)); |
| 23 | + private LEDPattern autoplace = |
| 24 | + LEDPattern.steps(Map.of(LEDConstants.stripLength / 3 * 2, LEDConstants.kAutoPlacing)) |
| 25 | + .blink(Seconds.of(0.25)); |
| 26 | + public LEDStates currState = LEDStates.OFF; |
| 27 | + public CoralStates coralState = CoralStates.NO_PIECE; |
| 28 | + public boolean autoPlacing = false; |
| 29 | + public boolean hasAlgae = false; |
| 30 | + public boolean isLimiting = false; |
20 | 31 |
|
21 | | - private LEDPattern currentLimiting = LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(1), Seconds.of(2)); |
22 | | - private LEDPattern autoplace = LEDPattern.steps(Map.of(LEDConstants.stripLength/3*2, LEDConstants.kAutoPlacing)).blink(Seconds.of(0.25)); |
23 | | - public LEDStates currState = LEDStates.OFF; |
24 | | - public CoralStates coralState = CoralStates.NO_PIECE; |
25 | | - public boolean autoPlacing = false; |
26 | | - public boolean hasAlgae = false; |
27 | | - public boolean isLimiting = false; |
28 | | - |
29 | | - public LEDSubsystem(LEDIO io) { |
30 | | - this.io = io; |
31 | | - } |
| 32 | + public LEDSubsystem(LEDIO io) { |
| 33 | + this.io = io; |
| 34 | + } |
32 | 35 |
|
33 | | - private void buildBase() { |
34 | | - switch (coralState) { |
35 | | - case IN_FUNNEL: |
36 | | - coral = LEDPattern.solid(LEDConstants.kCoralInFunnel); |
37 | | - break; |
38 | | - case IN_ROBOT: |
39 | | - coral = LEDPattern.solid(LEDConstants.kCoralInRobot); |
40 | | - break; |
41 | | - case NO_PIECE: |
42 | | - coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); |
43 | | - break; |
44 | | - } |
45 | | - algea = LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength/3, Color.kBlack)); |
46 | | - |
| 36 | + private void buildBase() { |
| 37 | + switch (coralState) { |
| 38 | + case IN_FUNNEL: |
| 39 | + coral = LEDPattern.solid(LEDConstants.kCoralInFunnel); |
| 40 | + break; |
| 41 | + case IN_ROBOT: |
| 42 | + coral = LEDPattern.solid(LEDConstants.kCoralInRobot); |
| 43 | + break; |
| 44 | + case NO_PIECE: |
| 45 | + coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); |
| 46 | + break; |
47 | 47 | } |
| 48 | + algea = |
| 49 | + LEDPattern.steps( |
| 50 | + Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength / 3, Color.kBlack)); |
| 51 | + } |
48 | 52 |
|
49 | | - public void periodic() { |
50 | | - switch (currState) { |
51 | | - case OFF: |
52 | | - break; |
53 | | - case NORMAL: |
54 | | - if (autoPlacing) { |
55 | | - base = autoplace.overlayOn(base); |
56 | | - } |
57 | | - if (isLimiting) { |
58 | | - base = currentLimiting.overlayOn(base); |
59 | | - } |
60 | | - io.setStrip(base); |
61 | | - break; |
62 | | - case CLIMB: |
63 | | - break; |
| 53 | + public void periodic() { |
| 54 | + switch (currState) { |
| 55 | + case OFF: |
| 56 | + break; |
| 57 | + case NORMAL: |
| 58 | + if (autoPlacing) { |
| 59 | + base = autoplace.overlayOn(base); |
| 60 | + } |
| 61 | + if (isLimiting) { |
| 62 | + base = currentLimiting.overlayOn(base); |
64 | 63 | } |
65 | | - io.updateLEDs(); |
| 64 | + io.setStrip(base); |
| 65 | + break; |
| 66 | + case CLIMB: |
| 67 | + break; |
66 | 68 | } |
| 69 | + io.updateLEDs(); |
| 70 | + } |
67 | 71 |
|
68 | | - @Override |
69 | | - public Set<Measure> getMeasures() { |
70 | | - return Set.of(new Measure("State", "the current Overall state of the LEDSubsystem", () -> currState.ordinal()), |
71 | | - new Measure("CoralState", "the current Coral state of the LEDSubsystem", () -> coralState.ordinal())); |
72 | | - } |
| 72 | + @Override |
| 73 | + public Set<Measure> getMeasures() { |
| 74 | + return Set.of( |
| 75 | + new Measure( |
| 76 | + "State", "the current Overall state of the LEDSubsystem", () -> currState.ordinal()), |
| 77 | + new Measure( |
| 78 | + "CoralState", |
| 79 | + "the current Coral state of the LEDSubsystem", |
| 80 | + () -> coralState.ordinal())); |
| 81 | + } |
73 | 82 |
|
74 | | - public enum LEDStates { |
75 | | - OFF, |
76 | | - NORMAL, |
77 | | - CLIMB |
78 | | - } |
| 83 | + public enum LEDStates { |
| 84 | + OFF, |
| 85 | + NORMAL, |
| 86 | + CLIMB |
| 87 | + } |
79 | 88 |
|
80 | | - public enum CoralStates { |
81 | | - IN_FUNNEL, |
82 | | - IN_ROBOT, |
83 | | - NO_PIECE, |
84 | | - } |
| 89 | + public enum CoralStates { |
| 90 | + IN_FUNNEL, |
| 91 | + IN_ROBOT, |
| 92 | + NO_PIECE, |
| 93 | + } |
85 | 94 | } |
0 commit comments