diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 56b6ebe..4ae5550 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -245,7 +245,7 @@ public static class LightStrips { public static final int UNDERGLOW_LED_COUNT = 150; /** The speed of the fade animation. [0, 1] */ - public static final double STRIP_FADE_AMOUNT = 0.1; + public static final double STRIP_FADE_AMOUNT = 0.05; public static final class Colors { /** The color for the lights when the robot is disabled. */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a4026c7..64a4268 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,6 +45,7 @@ public void disabledPeriodic() {} @Override public void disabledExit() { RobotContainer.mailboxPneumatics.retract(); + RobotContainer.disabledLights.cancel(); } @Override diff --git a/src/main/java/frc/robot/commands/lightstrip/DisabledLights.java b/src/main/java/frc/robot/commands/lightstrip/DisabledLights.java index 80740b3..823163b 100644 --- a/src/main/java/frc/robot/commands/lightstrip/DisabledLights.java +++ b/src/main/java/frc/robot/commands/lightstrip/DisabledLights.java @@ -11,6 +11,7 @@ package frc.robot.commands.lightstrip; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.RobotContainer; @@ -19,6 +20,9 @@ /** Activates when the robot is disabled. */ public class DisabledLights extends Command { private AddressableLightStrip robotLights; + private boolean firstStart = true; + private int rainbowTick = 0; + private int ledCount = 1; /** Creates a new RobotDisabledLights. */ public DisabledLights() { @@ -29,18 +33,37 @@ public DisabledLights() { // Called when the command is initially scheduled. @Override public void initialize() { - robotLights.setStripColor(Constants.LightStrips.Colors.DISABLED_COLOR); - robotLights.flush(); + if (!firstStart) { + robotLights.setStripColor(Constants.LightStrips.Colors.DISABLED_COLOR); + robotLights.flush(); + } robotLights.startLights(); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (firstStart) { + rainbowTick++; + for (int led = 0; led < ledCount; led++) { + robotLights.setColorLight( + led, Color.fromHSV((led + rainbowTick) % 180, 255, rainbowTick - led)); + } + robotLights.flush(); + if ((rainbowTick >= robotLights.getLength() * 2)) { + firstStart = false; + robotLights.setStripColorRaw(Color.kGhostWhite); + robotLights.setStripColor(Constants.LightStrips.Colors.DISABLED_COLOR); + } + ledCount = Math.min(ledCount + 1, robotLights.getLength()); + } + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + firstStart = false; + } @Override public boolean runsWhenDisabled() { @@ -50,6 +73,6 @@ public boolean runsWhenDisabled() { // Returns true when the command should end. @Override public boolean isFinished() { - return true; + return false; } } diff --git a/src/main/java/frc/robot/subsystems/AddressableLightStrip.java b/src/main/java/frc/robot/subsystems/AddressableLightStrip.java index 3d63dd1..5b304de 100644 --- a/src/main/java/frc/robot/subsystems/AddressableLightStrip.java +++ b/src/main/java/frc/robot/subsystems/AddressableLightStrip.java @@ -84,6 +84,17 @@ public void setStripColor(Color color) { targetStripColor = color; } + /** + * Directly sets the LED colors with no fade effect. + * + * @param color The color to set the LEDs to. + */ + public void setStripColorRaw(Color color) { + for (int led = 0; led < this.buffer.getLength(); led++) { + this.buffer.setLED(led, color); + } + } + private static double lerp(double v0, double v1, double amount) { if (v0 == v1) { return v0;