Add LEDs

src/main/java/org/team1540/robot2024/Constants.java

@@ -23,7 +23,10 @@ public final class Constants {
     private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY
 
     public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;
-
+    public static final class Leds {
+        public static final int LED_STRIP_PORT_PWM = 9;
+        public static final int LED_STRIP_LENGTH= 80;
+    }
     public enum Mode {
         /**
          * Running on a real robot.

src/main/java/org/team1540/robot2024/Robot.java

@@ -1,6 +1,7 @@
 package org.team1540.robot2024;
 
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import org.littletonrobotics.junction.LogFileUtil;
@@ -9,6 +10,12 @@
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+import org.team1540.robot2024.subsystems.led.*;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternRainbow;
+import org.team1540.robot2024.subsystems.led.patterns.SimpleLedPattern;
+import org.team1540.robot2024.util.LoggedTunableNumber;
 import org.team1540.robot2024.util.MechanismVisualiser;
 
 /**
@@ -21,6 +28,10 @@ public class Robot extends LoggedRobot {
     private Command autonomousCommand;
     private RobotContainer robotContainer;
 
+    LoggedTunableNumber led_r = new LoggedTunableNumber("led/r", 0);
+    LoggedTunableNumber led_g = new LoggedTunableNumber("led/g", 0);
+    LoggedTunableNumber led_b = new LoggedTunableNumber("led/b", 0);
+
     /**
      * This function is run when the robot is first started up and should be used for any
      * initialization code.
@@ -49,7 +60,7 @@ public void robotInit() {
         switch (Constants.currentMode) {
             case REAL:
                 // Running on a real robot, log to a USB stick ("/U/logs")
-                Logger.addDataReceiver(new WPILOGWriter());
+//                Logger.addDataReceiver(new WPILOGWriter());
                 Logger.addDataReceiver(new NT4Publisher());
                 break;
 
@@ -94,13 +105,15 @@ public void robotPeriodic() {
 
         // Update mechanism visualiser in sim
         if (Robot.isSimulation()) MechanismVisualiser.periodic();
+//        robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.alternating(Color.kBlueViolet, Color.kGreen));
     }
 
     /**
      * This function is called once when the robot is disabled.
      */
     @Override
     public void disabledInit() {
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK, new LedPatternRainbow(1));
     }
 
     /**
@@ -115,6 +128,7 @@ public void disabledPeriodic() {
      */
     @Override
     public void autonomousInit() {
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,LedPatternRSLState.matchingColors());
         autonomousCommand = robotContainer.getAutonomousCommand();
         // schedule the autonomous command (example)
         if (autonomousCommand != null) {
@@ -138,6 +152,8 @@ public void teleopInit() {
         // teleop starts running. If you want the autonomous to
         // continue until interrupted by another command, remove
         // this line or comment it out.
+
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK, LedPatternRSLState.matchingColors());
         if (autonomousCommand != null) {
             autonomousCommand.cancel();
         }
@@ -155,6 +171,7 @@ public void teleopPeriodic() {
      */
     @Override
     public void testInit() {
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,new LedPatternRainbow(1));
         // Cancels all running commands at the start of test mode.
         CommandScheduler.getInstance().cancelAll();
     }
@@ -164,6 +181,7 @@ public void testInit() {
      */
     @Override
     public void testPeriodic() {
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,SimpleLedPattern.solid(new Color(led_r.get(), led_g.get(), led_b.get())));
     }
 
     /**

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -3,13 +3,18 @@
 import com.pathplanner.lib.util.PathPlannerLogging;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
@@ -30,6 +35,8 @@
 import org.team1540.robot2024.subsystems.indexer.IndexerIO;
 import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
 import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
+import org.team1540.robot2024.subsystems.led.Leds;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
 import org.team1540.robot2024.subsystems.shooter.*;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
 import org.team1540.robot2024.subsystems.tramp.TrampIO;
@@ -61,6 +68,7 @@ public class RobotContainer {
     public final Elevator elevator;
     public final Indexer indexer;
     public final AprilTagVision aprilTagVision;
+    public final Leds leds = new Leds();
 
     private int currentPathHash = Integer.MAX_VALUE;
     private Trajectory currentPathTrajectory = new Trajectory();
@@ -208,8 +216,17 @@ public RobotContainer() {
 
         // Configure the button bindings
         configureButtonBindings();
+        configureLedBindings();
     }
 
+    private void configureLedBindings() {
+        leds.setFatalPattern(LedPatternFlame::new);
+        new Trigger(DriverStation::isDSAttached)
+                .onTrue(Commands.runOnce(leds::clearFatalPattern)
+                            .ignoringDisable(true))
+                .onFalse(Commands.runOnce(() -> leds.setFatalPattern(LedPatternFlame::new))
+                            .ignoringDisable(true));
+    }
     /**
      * Use this method to define your button->command mappings. Buttons can be created by
      * instantiating a {@link GenericHID} or one of its subclasses ({@link

src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java

@@ -0,0 +1,38 @@
+package org.team1540.robot2024.subsystems.led;
+
+import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternRainbow;
+
+
+public class LedTriager {
+    private final LedPattern[] patterns = new LedPattern[Leds.CRITICALITY_COUNT];
+    private final LedPattern defaultPattern = new LedPatternRainbow(1);
+    private boolean isNew = true;
+    public LedPattern getPattern() {
+        for (int i = patterns.length -1; i >= 0; i--) {
+            if (patterns[i] != null) {
+                return patterns[i];
+            }
+        }
+        return defaultPattern;
+    }
+
+    public boolean shouldRefresh() {
+        final boolean val = isNew || getPattern().isDynamic();
+        isNew = false;
+        return val;
+    }
+
+    public void clearPattern(Leds.PatternCriticality criticality) {
+        patterns[criticality.ordinal()] = null;
+        isNew = true;
+    }
+
+    public boolean addPattern(LedPattern pattern, Leds.PatternCriticality criticality) {
+        patterns[criticality.ordinal()] = pattern;
+        isNew = true;
+        return getPattern() == pattern;
+    }
+
+}

src/main/java/org/team1540/robot2024/subsystems/led/Leds.java

@@ -0,0 +1,80 @@
+package org.team1540.robot2024.subsystems.led;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
+
+import java.util.function.Supplier;
+
+import static org.team1540.robot2024.Constants.Leds.*;
+
+public class Leds extends SubsystemBase {
+    private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(LED_STRIP_LENGTH);
+    private final AddressableLED strip = new AddressableLED(LED_STRIP_PORT_PWM);
+    private final ZonedAddressableLEDBuffer[] buffers = new ZonedAddressableLEDBuffer[ZONE_COUNT];
+    private final LedTriager[] patterns = new LedTriager[ZONE_COUNT];
+
+    public Leds() {
+        strip.setLength(ledBuffer.getLength());
+        strip.setData(ledBuffer);
+        strip.start();
+
+        buffers[Zone.ELEVATOR_BACK.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 41, false);
+        for (int i = 0; i < ZONE_COUNT;i++) {
+            patterns[i] = new LedTriager();
+        }
+    }
+
+    @Override
+    public void periodic() {
+        for (int i = 0; i < ZONE_COUNT;i++) {
+            if (patterns[i].shouldRefresh()) {
+                patterns[i].getPattern().apply(buffers[i]);
+            }
+        }
+        strip.setData(ledBuffer);
+    }
+
+    public void setPattern(Zone zone, LedPattern pattern, PatternCriticality criticality) {
+        patterns[zone.ordinal()].addPattern(pattern, criticality);
+        pattern.setLength(buffers[zone.ordinal()].getLength());
+    }
+
+    public void setPattern(Zone zone, LedPattern pattern) {
+        setPattern(zone, pattern, PatternCriticality.INFO);
+    }
+
+    public void clearPattern(Zone zone, PatternCriticality criticality) {
+        patterns[zone.ordinal()].clearPattern(criticality);
+    }
+
+    public void setFatalPattern(Supplier<LedPattern> patternSupplier) {
+        for (int i = 0; i<ZONE_COUNT;i++) {
+            LedPattern pattern = patternSupplier.get();
+            patterns[i].addPattern(pattern, PatternCriticality.FATAL);
+            pattern.setLength(buffers[i].getLength());
+        }
+    }
+
+    public void clearFatalPattern() {
+        for (int i = 0; i<ZONE_COUNT;i++) {
+            patterns[i].clearPattern(PatternCriticality.FATAL);
+        }
+    }
+
+
+    private static final int ZONE_COUNT=Zone.values().length;
+    public enum Zone {
+        ELEVATOR_BACK,
+    }
+    static final int CRITICALITY_COUNT=PatternCriticality.values().length;
+    public enum PatternCriticality {
+        DECORATION,
+        INFO,
+        CRITICAL,
+        FATAL
+    }
+
+}

src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java

@@ -0,0 +1,63 @@
+package org.team1540.robot2024.subsystems.led;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+public class ZonedAddressableLEDBuffer {
+    private final boolean isInverted;
+    private final int start;
+    private final int length;
+    private final AddressableLEDBuffer buffer;
+
+    public ZonedAddressableLEDBuffer(AddressableLEDBuffer buffer, int start, int end, boolean isInverted) {
+        if (start > end) {
+            throw new IllegalArgumentException("start must be less than end");
+        }
+        this.buffer = buffer;
+        this.isInverted = isInverted;
+        this.start = start;
+        this.length = end - start;
+    }
+
+    public void setRGB(int index, int r, int g, int b) {
+        buffer.setRGB(this.getAbsoluteIndex(index), r, g, b);
+    }
+
+    public void setHSV(int index, int h, int s, int v) {
+        buffer.setHSV(this.getAbsoluteIndex(index), h, s, v);
+    }
+
+    public void setLED(int index, Color color) {
+        buffer.setLED(this.getAbsoluteIndex(index), color);
+    }
+
+    public void setLED(int index, Color8Bit color) {
+        buffer.setLED(this.getAbsoluteIndex(index), color);
+    }
+
+    public Color getLED(int index) {
+        return buffer.getLED(this.getAbsoluteIndex(index));
+    }
+
+    public Color8Bit getLED8Bit(int index) {
+        return buffer.getLED8Bit(this.getAbsoluteIndex(index));
+    }
+
+    public int getLength() {
+        return this.length;
+    }
+
+    private int getAbsoluteIndex(int index) {
+        if (index >= length) {
+            DriverStation.reportWarning("led index out of bounds", false);
+            return 0;
+        }
+        if (this.isInverted) {
+            return this.start + length - 1 - index;
+        } else {
+            return this.start + index;
+        }
+    }
+}

src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPattern.java

@@ -0,0 +1,47 @@
+package org.team1540.robot2024.subsystems.led.patterns;
+
+import edu.wpi.first.wpilibj.util.Color;
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
+
+public abstract class LedPattern {
+    private final boolean isDynamic;
+
+    protected LedPattern(boolean isDynamic) {
+        this.isDynamic = isDynamic;
+    }
+
+    public final boolean isDynamic() {
+        return isDynamic;
+    }
+
+    public abstract void apply(ZonedAddressableLEDBuffer buffer);
+    public void setLength(int length) {}
+
+    protected static int getHue(Color color) {
+        final int red = (int) color.red * 255;
+        final int green = (int) color.green * 255;
+        final int blue = (int) color.blue * 255;
+        float min = Math.min(Math.min(red, green), blue);
+        float max = Math.max(Math.max(red, green), blue);
+
+        if (min == max) {
+            return 0;
+        }
+
+        float hue = 0f;
+        if (max == red) {
+            hue = (green - blue) / (max - min);
+
+        } else if (max == green) {
+            hue = 2f + (blue - red) / (max - min);
+
+        } else {
+            hue = 4f + (red - green) / (max - min);
+        }
+
+        hue = hue * 60;
+        if (hue < 0) hue = hue + 360;
+
+        return Math.round(hue);
+    }
+}