added an led subsystem

example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/led/LEDStatusLight.java

@@ -29,8 +29,8 @@ public class LEDStatusLight extends MapleSubsystem {
     public LEDStatusLight(int port, int length) {
         super("LED");
         if (led != null) led.close();
-        led = Robot.CURRENT_ROBOT_MODE == RobotMode.REAL ? new AddressableLED(port) : null;
-        if (led != null) led.setLength(length);
+        led = new AddressableLED(port);
+        led.setLength(length);
         this.buffer = new AddressableLEDBuffer(length);
         this.bufferForDashboard = new AddressableLEDBuffer(DASHBOARD_DISPLAY_LENGTH);
         this.animation = DISABLED;

src/main/java/frc/robot/Robot.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.constants.RobotMode;
 import frc.robot.subsystems.MapleSubsystem;
+import frc.robot.subsystems.led.LEDAnimation;
 import org.ironmaple.simulation.SimulatedArena;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
@@ -78,6 +79,13 @@ public void robotInit() {
     public void robotPeriodic() {
         MapleSubsystem.checkForOnDisableAndEnable();
         CommandScheduler.getInstance().run();
+        if (robotContainer.drive.hardwareFaultsDetected.getAsBoolean())
+            robotContainer
+                    .ledStatusLight
+                    .playAnimationPeriodically(new LEDAnimation.Breathe(255, 0, 0), 2)
+                    .until(robotContainer.drive.hardwareFaultsDetected.negate())
+                    .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)
+                    .schedule();
     }
 
     /** This function is called once when the robot is disabled. */

src/main/java/frc/robot/RobotContainer.java

@@ -22,6 +22,8 @@
 import frc.robot.subsystems.MapleSubsystem;
 import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.drive.IO.*;
+import frc.robot.subsystems.led.LEDAnimation;
+import frc.robot.subsystems.led.LEDStatusLight;
 import frc.robot.subsystems.vision.apriltags.AprilTagVision;
 import frc.robot.subsystems.vision.apriltags.AprilTagVisionIOReal;
 import frc.robot.subsystems.vision.apriltags.ApriltagVisionIOSim;
@@ -52,6 +54,7 @@ public class RobotContainer {
     // Subsystems
     public final SwerveDrive drive;
     public final AprilTagVision aprilTagVision;
+    public final LEDStatusLight ledStatusLight;
 
     /* an example shooter optimization */
     public final MapleShooterOptimization exampleShooterOptimization;
@@ -169,6 +172,8 @@ public RobotContainer() {
             }
         }
 
+        this.ledStatusLight = new LEDStatusLight(0, 155);
+
         this.drive.configHolonomicPathPlannerAutoBuilder();
 
         SmartDashboard.putData("Select Test", testChooser = buildTestsChooser());
@@ -188,6 +193,7 @@ public RobotContainer() {
 
         configureButtonBindings();
         configureAutoNamedCommands();
+        configureLEDEffects();
     }
 
     private void configureAutoNamedCommands() {
@@ -318,6 +324,10 @@ public void configureButtonBindings() {
         driverXBox.b().whileTrue(exampleAutoAlignment);
     }
 
+    public void configureLEDEffects() {
+        ledStatusLight.setDefaultCommand(ledStatusLight.showEnableDisableState());
+    }
+
     /**
      * Use this to pass the autonomous command to the main {@link Robot} class.
      *

src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java

@@ -61,6 +61,10 @@ public ModuleIOSim(SwerveModuleSimulation moduleSimulation) {
 
     @Override
     public void updateInputs(ModuleIOInputs inputs) {
+        inputs.driveMotorConnected = true;
+        inputs.steerMotorConnected = true;
+        inputs.steerEncoderConnected = true;
+
         inputs.driveWheelFinalRevolutions =
                 moduleSimulation.getDriveWheelFinalPosition().in(Revolutions);
         inputs.driveWheelFinalVelocityRevolutionsPerSec =

src/main/java/frc/robot/utils/LEDAnimation.java → src/main/java/frc/robot/subsystems/led/LEDAnimation.java

@@ -1,26 +1,28 @@
-package frc.robot.utils;
+package frc.robot.subsystems.led;
 
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import org.ironmaple.utils.mathutils.MapleCommonMath;
 
-/** interface for an LED animation that can be displayed on the dashboard or by an addressable led */
-public interface LEDAnimation {
+public sealed interface LEDAnimation {
+    /**
+     * Plays one frame the animation
+     *
+     * @param buffer the {@link AddressableLEDBuffer} to play on
+     * @param t the time of the frame. 0~1 represents a full animation cycle
+     */
     void play(AddressableLEDBuffer buffer, double t);
 
     final class Breathe implements LEDAnimation {
         private final int colorR, colorG, colorB;
-        private final double hz;
 
-        public Breathe(int colorR, int colorG, int colorB, double hz) {
+        public Breathe(int colorR, int colorG, int colorB) {
             this.colorR = colorR;
             this.colorG = colorG;
             this.colorB = colorB;
-            this.hz = hz;
         }
 
         @Override
         public void play(AddressableLEDBuffer buffer, double t) {
-            t *= hz;
             final double brightness = 0.5 + 0.5 * Math.sin(t * Math.PI);
             for (int i = 0; i < buffer.getLength(); i++)
                 buffer.setRGB(i, (int) (colorR * brightness), (int) (colorG * brightness), (int) (colorB * brightness));
@@ -43,65 +45,55 @@ public void play(AddressableLEDBuffer buffer, double t) {
     }
 
     final class SlideBackAndForth extends Slide {
-        private final double hz1;
-
-        public SlideBackAndForth(int colorR, int colorG, int colorB, double hz, double slideLength) {
-            super(colorR, colorG, colorB, 1, slideLength);
-            this.hz1 = hz;
+        public SlideBackAndForth(int colorR, int colorG, int colorB, double slideLength) {
+            super(colorR, colorG, colorB, slideLength);
         }
 
         @Override
         public void play(AddressableLEDBuffer buffer, double t) {
-            super.play(buffer, 0.5 + 0.5 * Math.sin(t * hz1));
+            super.play(buffer, 0.5 + 0.5 * Math.sin(t * Math.PI));
         }
     }
 
-    class Slide implements LEDAnimation {
+    sealed class Slide implements LEDAnimation {
         private final int colorR, colorG, colorB;
-        private final double hz, slideLength;
+        private final double slideLength;
 
-        public Slide(int colorR, int colorG, int colorB, double hz, double slideLength) {
+        public Slide(int colorR, int colorG, int colorB, double slideLength) {
             this.colorR = colorR;
             this.colorG = colorG;
             this.colorB = colorB;
-            this.hz = hz;
             this.slideLength = slideLength;
         }
 
         @Override
         public void play(AddressableLEDBuffer buffer, double t) {
-            t *= hz;
-            t %= 1;
-            final double lowerEdge = MapleCommonMath.linearInterpretation(0, -slideLength, 1, 1, t),
-                    upperEdge = lowerEdge + slideLength;
+            int halfLength = buffer.getLength() / 2;
+            double lowerEdge = MapleCommonMath.linearInterpretation(0, -slideLength, 1, 1, t);
+            double upperEdge = lowerEdge + slideLength;
             /* strip is half the entire led */
-            final int stripLength = buffer.getLength() / 2;
-            for (int i = 0; i < stripLength; i++) {
+            for (int i = 0; i < halfLength; i++) {
                 int r = colorR, g = colorG, b = colorB;
-                if (i >= (int) (upperEdge * stripLength) || i <= (int) (lowerEdge * stripLength)) r = g = b = 0;
-                buffer.setRGB(stripLength + i, r, g, b);
-                buffer.setRGB(stripLength - i - 1, r, g, b);
+                if (i >= (int) (upperEdge * halfLength) || i <= (int) (lowerEdge * halfLength)) r = g = b = 0;
+                buffer.setRGB(halfLength + i, r, g, b);
+                buffer.setRGB(halfLength - i - 1, r, g, b);
             }
         }
     }
 
     final class Charging implements LEDAnimation {
         private final int colorR, colorG, colorB;
-        private final double hz;
 
-        public Charging(int colorR, int colorG, int colorB, double hz) {
+        public Charging(int colorR, int colorG, int colorB) {
             this.colorR = colorR;
             this.colorG = colorG;
             this.colorB = colorB;
-            this.hz = hz;
         }
 
         @Override
         public void play(AddressableLEDBuffer buffer, double t) {
-            t *= hz;
-            t %= 1;
             final int edge = (int) (t * buffer.getLength() / 2);
-            final double coolDownTime = 0.5 * hz;
+            final double coolDownTime = 0.2;
 
             t *= 1 + coolDownTime;
             for (int i = 0; i < buffer.getLength() / 2; i++) {
@@ -118,45 +110,9 @@ public void play(AddressableLEDBuffer buffer, double t) {
         }
     }
 
-    class ChargingDualColor implements LEDAnimation {
-        private final int fromColorR, fromColorG, fromColorB, toColorR, toColorG, toColorB;
-        private final double duration;
-
-        public ChargingDualColor(
-                int fromColorR,
-                int fromColorG,
-                int fromColorB,
-                int toColorR,
-                int toColorG,
-                int toColorB,
-                double duration) {
-            this.fromColorR = fromColorR;
-            this.fromColorG = fromColorG;
-            this.fromColorB = fromColorB;
-            this.toColorR = toColorR;
-            this.toColorG = toColorG;
-            this.toColorB = toColorB;
-            this.duration = duration;
-        }
-
-        @Override
-        public void play(AddressableLEDBuffer buffer, double t) {
-            t = Math.min(Math.max(t / duration, 0), 1);
-
-            // TODO charging animation
-        }
-    }
-
     final class Rainbow implements LEDAnimation {
-        private final double hz;
-
-        public Rainbow(double hz) {
-            this.hz = hz;
-        }
-
         @Override
         public void play(AddressableLEDBuffer buffer, double t) {
-            t *= hz;
             final int firstPixelHue = (int) (t * 180), v = 128;
             for (var i = 0; i < buffer.getLength() / 2; i++) {
                 final int colorH = (firstPixelHue + (i * 180 / buffer.getLength())) % 180;
@@ -165,22 +121,4 @@ public void play(AddressableLEDBuffer buffer, double t) {
             }
         }
     }
-
-    final class PoliceLight implements LEDAnimation {
-        private final double hz;
-
-        public PoliceLight(double hz) {
-            this.hz = hz;
-        }
-
-        @Override
-        public void play(AddressableLEDBuffer buffer, double t) {
-            t *= hz;
-            for (int i = 0; i < buffer.getLength() / 2; i++) {
-                final int blink = t > 0.5 ? 255 : 0;
-                buffer.setRGB(i, t > 0.5 ? 255 : 0, 0, 0);
-                buffer.setRGB(buffer.getLength() - i - 1, 0, 0, t < 0.5 ? 255 : 0);
-            }
-        }
-    }
 }

src/main/java/frc/robot/subsystems/led/LEDStatusLight.java

@@ -0,0 +1,70 @@
+package frc.robot.subsystems.led;
+
+import edu.wpi.first.wpilibj.*;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import frc.robot.Robot;
+import frc.robot.subsystems.MapleSubsystem;
+import org.littletonrobotics.junction.Logger;
+
+public class LEDStatusLight extends MapleSubsystem {
+    private static final int DASHBOARD_DISPLAY_LENGTH = 32;
+    private static AddressableLED led = null;
+    private final AddressableLEDBuffer buffer;
+    private final AddressableLEDBuffer bufferForDashboard;
+
+    public LEDStatusLight(int port, int length) {
+        super("LED");
+        this.buffer = new AddressableLEDBuffer(length);
+        this.bufferForDashboard = new AddressableLEDBuffer(DASHBOARD_DISPLAY_LENGTH);
+
+        if (led != null) led.close();
+        led = new AddressableLED(port);
+        led.setLength(length);
+        led.setData(buffer);
+        led.start();
+    }
+
+    final String[] dashboardColors = new String[DASHBOARD_DISPLAY_LENGTH];
+
+    @Override
+    public void periodic(double dt, boolean enabled) {
+        for (int i = 0; i < DASHBOARD_DISPLAY_LENGTH; i++)
+            dashboardColors[i] = bufferForDashboard.getLED(i).toHexString();
+
+        led.setData(buffer);
+        if (!Robot.isReal()) Logger.recordOutput("Status Light", dashboardColors);
+    }
+
+    public Command playAnimation(LEDAnimation animation, double timeSeconds) {
+        Timer timer = new Timer();
+        timer.start();
+        return this.run(() -> {
+                    double t = timer.get() / timeSeconds;
+                    animation.play(buffer, t);
+                    animation.play(bufferForDashboard, t);
+                })
+                .beforeStarting(timer::reset)
+                .withTimeout(timeSeconds)
+                .ignoringDisable(true);
+    }
+
+    public Command playAnimation(LEDAnimation animation, double timeSeconds, int loopNum) {
+        return playAnimation(animation, timeSeconds).repeatedly().withTimeout(timeSeconds * loopNum);
+    }
+
+    public Command playAnimationPeriodically(LEDAnimation animation, double hz) {
+        double timeSeconds = 1.0 / hz;
+        return this.playAnimation(animation, timeSeconds).repeatedly().ignoringDisable(true);
+    }
+
+    public Command showEnableDisableState() {
+        return new ConditionalCommand(
+                        playAnimation(new LEDAnimation.SlideBackAndForth(0, 200, 255, 0.8), 0.8)
+                                .until(RobotState::isDisabled),
+                        playAnimation(new LEDAnimation.Breathe(0, 200, 255), 2).until(RobotState::isEnabled),
+                        RobotState::isEnabled)
+                .repeatedly()
+                .ignoringDisable(true);
+    }
+}