diff --git a/leds/LightningColors.java b/leds/LightningColors.java index 130c659..a3d355c 100644 --- a/leds/LightningColors.java +++ b/leds/LightningColors.java @@ -4,8 +4,8 @@ public enum LightningColors { BLACK(0, 0, 0), WHITE(0, 255, 255), RED(0, 255, 255), - ORANGE(15, 255, 255), - YELLOW(30, 255, 255), + ORANGE(5, 255, 255), + YELLOW(15, 255, 255), GREEN(60, 255, 255), LIGHT_BLUE(100, 255, 255), BLUE(120, 255, 255), diff --git a/leds/ThunderStrip.java b/leds/ThunderStrip.java index f9fde96..e2eccc0 100644 --- a/leds/ThunderStrip.java +++ b/leds/ThunderStrip.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.LEDConstants.LEDStates; @@ -48,21 +49,47 @@ public Command enableState(LEDStates state) { } /** - * @param state the state to enable + * @param state the state to enable * @param duration the duration to enable the state for - * @return a command that enables the state - */ - public Command enableStateFor(LEDStates state, int duration) { - return new StartEndCommand(() -> { - if (!unusedStates.contains(state)) { + * @return a command that enables the state + */ + public Command enableStateFor(LEDStates state, int duration) { + return new StartEndCommand(() -> { + if (!unusedStates.contains(state)) { states.add(state); } - }, () -> { - states.remove(state); - }) + }, () -> { + states.remove(state); + }) .withDeadline(new WaitCommand(duration)) .ignoringDisable(true); - } + } + + /** + * Sets the current state of the LED strip + * + * @param state the state to set + * @param enabled whether to enable or disable the state + * @return an InstantCommand that sets the state + */ + public Command setState(LEDStates state, boolean enabled) { + return new InstantCommand(() -> { + if (enabled) { + if (!unusedStates.contains(state)) { + states.add(state); + } + } else { + states.remove(state); + } + }); + } + + /** + * @return the current state of the LED strip + */ + public LEDStates getState() { + return states.peek(); + } /** * Updates the LED strip @@ -71,7 +98,7 @@ public void update() { if (states.isEmpty()) { defaultLEDs(); } else { - updateLEDs(states.peek()); + updateLEDs(getState()); } } @@ -126,7 +153,7 @@ public void swirl(LightningColors color1, LightningColors color2, int segmentSiz * @param hue the hue to blink */ public void blink(LightningColors color) { - if ((int) (Timer.getFPGATimestamp() * 10) % 2 == 0) { + if ((int) (Timer.getFPGATimestamp() * 20) % 2 == 0) { // Increased the multiplier to 20 for faster strobe leds.setSolidHSV(color, length, startIndex); } else { leds.setSolidHSV(LightningColors.BLACK, length, startIndex);