Add LED subsystem and commands

src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java

@@ -27,6 +27,7 @@
 import frc.robot.subsystems.controls.drive.DriveControls;
 import frc.robot.subsystems.controls.elevator.ElevatorControls;
 import frc.robot.subsystems.implementations.drive.DriveBase;
+import frc.robot.subsystems.implementations.led.LEDSubsystem;
 import frc.robot.subsystems.implementations.motor.ArmMotorSubsystem;
 import frc.robot.subsystems.implementations.motor.ElevatorMotorSubsystem;
 import frc.robot.subsystems.implementations.vision.VisionSubsystem;
@@ -42,6 +43,7 @@ public class RobotConfig {
   protected static ElevatorMotorSubsystem elevator;
   public static ArmMotorSubsystem coralArm;
   public static ArmMotorSubsystem climberArm;
+  public static LEDSubsystem leds;
 
   // Controls
   public CommandXboxController mainController = new CommandXboxController(0);
@@ -82,6 +84,9 @@ public RobotConfig(
       boolean stubCoralArm,
       boolean stubAlgaeSubsystem,
       boolean stubClimberArm) {
+
+    leds = new LEDSubsystem();
+
     if (stubDrive) {
       drive = new DriveBase("Stub");
     }

src/main/java/frc/robot/subsystems/controls/elevator/ElevatorControls.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems.controls.elevator;
 
+import edu.wpi.first.wpilibj.LEDPattern;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -8,6 +9,8 @@
 import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.commands.common.motor.MotorAutoResetEncoderCommand;
 import frc.robot.commands.common.motor.MotorAutoResetEncoderCommand.MotorAutoResetEncoderSettings;
+import frc.robot.config.game.reefscape2025.RobotConfig;
+import frc.robot.subsystems.implementations.led.LEDSubsystem;
 import frc.robot.subsystems.interfaces.Elevator;
 import frc.robot.subsystems.interfaces.Motor;
 
@@ -38,29 +41,44 @@ public static void setupController(Elevator elevator, CommandXboxController cont
             }));
 
     /* Add Auto Zero */
+
+    LEDSubsystem leds = RobotConfig.leds;
+
     Command autoCalibrateCommand =
-        new MotorAutoResetEncoderCommand((Motor) elevator, Constants.autoZeroSettings);
+        leds.redBlink()
+            .alongWith(
+                new MotorAutoResetEncoderCommand((Motor) elevator, Constants.autoZeroSettings))
+            .andThen(leds.greenBlink())
+            .withTimeout(2);
+    // leds.red()
+    // .andThen(new MotorAutoResetEncoderCommand((Motor) elevator, Constants.autoZeroSettings))
+    // .andThen(leds.green());
+
     SmartDashboard.putData(
         subsystem.getName() + "/Commands/Auto Calibrate Elevator", autoCalibrateCommand);
 
+    LEDPattern pattern =
+        LEDPattern.progressMaskLayer(
+            () -> elevator.getCurrentHeight() / elevator.getTargetHeight());
+
     SmartDashboard.putData(
         subsystem.getName() + "/Commands/Elevator To 0.3m\"",
-        new ElevatorToPosition(elevator, () -> 0.30));
+        leds.runPattern(pattern).alongWith(new ElevatorToPosition(elevator, () -> 0.30)));
     SmartDashboard.putData(
         subsystem.getName() + "/Commands/Elevator To 0.6m\"",
-        new ElevatorToPosition(elevator, () -> 0.60));
+        leds.runPattern(pattern).alongWith(new ElevatorToPosition(elevator, () -> 0.60)));
     SmartDashboard.putData(
         subsystem.getName() + "/Commands/Elevator To 0.8m\"",
-        new ElevatorToPosition(elevator, () -> 0.80));
+        leds.runPattern(pattern).alongWith(new ElevatorToPosition(elevator, () -> 0.80)));
     SmartDashboard.putData(
         subsystem.getName() + "/Commands/Elevator To 1.0m\"",
-        new ElevatorToPosition(elevator, () -> 1.0));
+        leds.runPattern(pattern).alongWith(new ElevatorToPosition(elevator, () -> 1.0)));
     SmartDashboard.putData(
         subsystem.getName() + "/Commands/Elevator To 1.2m\"",
-        new ElevatorToPosition(elevator, () -> 1.2));
+        leds.runPattern(pattern).alongWith(new ElevatorToPosition(elevator, () -> 1.2)));
     SmartDashboard.putData(
         subsystem.getName() + "/Commands/Elevator To 1.4m\"",
-        new ElevatorToPosition(elevator, () -> 1.4));
+        leds.runPattern(pattern).alongWith(new ElevatorToPosition(elevator, () -> 1.4)));
 
     SmartDashboard.putNumber(
         subsystem.getName() + "/Commands/Move To Position/Target", elevator.getCurrentHeight());

src/main/java/frc/robot/subsystems/implementations/led/LEDSubsystem.java

@@ -0,0 +1,174 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.implementations.led;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.LEDPattern;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class LEDSubsystem extends SubsystemBase {
+
+  private final AddressableLED led;
+  private final AddressableLEDBuffer ledBuffer;
+
+  private final LEDPattern orange = LEDPattern.solid(Color.kOrange);
+  private final LEDPattern red = LEDPattern.solid(Color.kRed);
+  private final LEDPattern green = LEDPattern.solid(Color.kGreen);
+  private final LEDPattern blue = LEDPattern.solid(Color.kBlue);
+  private final LEDPattern purple = LEDPattern.solid(Color.kPurple);
+
+  // Our LED strip has a density of 120 LEDs per meter
+  private static final Distance kLedSpacing = Meters.of(1 / 120.0);
+
+  // Create an LED pattern that will display a rainbow across
+  // all hues at maximum saturation and half brightness
+  private final LEDPattern m_rainbow = LEDPattern.rainbow(255, 128);
+
+  // Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a speed
+  // of 1 meter per second.
+  private final LEDPattern scrollingRainbow =
+      m_rainbow.scrollAtAbsoluteSpeed(MetersPerSecond.of(1), kLedSpacing);
+
+  /** Creates a new LEDSubsystem. */
+  public LEDSubsystem() {
+    led = new AddressableLED(0);
+    // Two strips of 60 leds joined together
+    ledBuffer = new AddressableLEDBuffer(120);
+    led.setLength(ledBuffer.getLength());
+
+    // AddressableLEDBufferView left = ledBuffer.createView(0, 30);
+    // AddressableLEDBufferView center = ledBuffer.createView(31, 90);
+    // AddressableLEDBufferView right = ledBuffer.createView(91, 119);
+
+    // Set the data
+    led.setData(ledBuffer);
+    led.start();
+
+    setDefaultCommand(idleMode());
+
+    String cmdPrefix = "LEDS/Commands/";
+
+    SmartDashboard.putData(cmdPrefix + "redLED", red());
+    SmartDashboard.putData(cmdPrefix + "blueLED", blue());
+    SmartDashboard.putData(cmdPrefix + "purpleLED", purple());
+
+    SmartDashboard.putData(cmdPrefix + "redBlinkLED", redBlink());
+    SmartDashboard.putData(cmdPrefix + "orangeBlinkLED", orangeBlink());
+    SmartDashboard.putData(cmdPrefix + "greenBlinkLED", greenBlink());
+
+    SmartDashboard.putData(this);
+  }
+
+  @Override
+  public void periodic() {
+
+    led.setData(ledBuffer); // Update the LED strip
+  }
+
+  private void setBlink(LEDPattern color, double interval) {
+    LEDPattern pattern = color.blink(Units.Seconds.of(interval));
+    pattern.applyTo(ledBuffer); // Apply  color
+    // led.setData(ledBuffer); // Update the LED strip
+  }
+
+  private void setOrangeBlink() {
+    setBlink(orange, .5);
+  }
+
+  private void setRedBlink() {
+    setBlink(red, .5);
+  }
+
+  private void setGreenBlink() {
+    setBlink(green, .5);
+  }
+
+  private void setPurpleBlink() {
+    setBlink(purple, .5);
+  }
+
+  private void setPurple() {
+    purple.applyTo(ledBuffer); // Apply  color
+  }
+
+  public Command runPattern(LEDPattern pattern) {
+    return new RunCommand(
+        () -> {
+          pattern.applyTo(ledBuffer);
+        },
+        this); // Ensure LEDSubsystem is required
+  }
+
+  public Command purple() {
+    return runPattern(purple).withName("purple");
+  }
+
+  public Command red() {
+    return runPattern(red).withName("red");
+  }
+
+  public Command blue() {
+    return runPattern(blue).withName("blue");
+  }
+
+  public Command green() {
+    return runPattern(green).withName("green");
+  }
+
+  public Command orangeBlink() {
+
+    return new RunCommand(
+            () -> {
+              setOrangeBlink();
+            },
+            this)
+        .withName("orangeBlink");
+  }
+
+  public Command redBlink() {
+    // this is to ensure LEDSubsystem is required
+    return new RunCommand(
+            () -> {
+              setRedBlink();
+            },
+            this)
+        .withName("redBlink");
+  }
+
+  public Command greenBlink() {
+    // this is to ensure LEDSubsystem is required
+    return new RunCommand(
+            () -> {
+              setGreenBlink();
+            },
+            this)
+        .withName("greenBlink");
+  }
+
+  public Command idleMode() {
+    return new RunCommand(
+            () -> {
+              if (DriverStation.isDisabled()) {
+                setOrangeBlink();
+              } else {
+                setPurpleBlink();
+              }
+            },
+            this)
+        .withName("ledIdle")
+        .ignoringDisable(true);
+  }
+}