added a ledSubsystem

and made robotState
use it

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

@@ -91,6 +91,7 @@
 import frc.robot.subsystems.elbow.ElbowSubsystem;
 import frc.robot.subsystems.intake.IntakeIOFX;
 import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.led.LedSubsystem;
 import frc.robot.subsystems.magazine.MagazineIOFX;
 import frc.robot.subsystems.magazine.MagazineSubsystem;
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
@@ -118,6 +119,7 @@ public class RobotContainer {
   private final ElbowSubsystem elbowSubsystem;
   private final ShooterSubsystem shooterSubsystem;
   private final ClimbSubsystem climbSubsystem;
+  private final LedSubsystem ledSubsystem;
   private final AutoSwitch autoSwitch;
 
   private final XboxController xboxController = new XboxController(1);
@@ -171,6 +173,7 @@ public RobotContainer() {
     shooterSubsystem = new ShooterSubsystem(shooterIO);
     intakeSubsystem = new IntakeSubsystem(intakeIO);
     magazineSubsystem = new MagazineSubsystem(magazineIO);
+    ledSubsystem = new LedSubsystem();
     climbSubsystem =
         new ClimbSubsystem(climbIO, new ClimbRatchetIOServo(), new TrapBarIOServo(), forkIO);
 
@@ -186,7 +189,8 @@ public RobotContainer() {
             intakeSubsystem,
             magazineSubsystem,
             superStructure,
-            climbSubsystem);
+            climbSubsystem,
+            ledSubsystem);
 
     driveSubsystem.setRobotStateSubsystem(robotStateSubsystem);
     autoSwitch =

src/main/java/frc/robot/constants/LedConstants.java

@@ -0,0 +1,10 @@
+package frc.robot.constants;
+
+import edu.wpi.first.wpilibj.util.Color;
+
+public class LedConstants {
+    public static final int kLedPort = 0; //FIXME led port unknown
+    public static final int kLedLength = 0; //FIXME led length unknown
+    public static final Color kBlue = new Color(42, 45, 232);
+    public static final Color kGreen = new Color(0, 255, 0);
+}

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

@@ -0,0 +1,98 @@
+package frc.robot.subsystems.led;
+
+import java.util.Random;
+import java.util.Set;
+
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.util.Color;
+import frc.robot.constants.LedConstants;
+
+public class LedSubsystem extends MeasurableSubsystem{
+
+    private LedState currState = LedState.OFF;
+    private AddressableLED led = new AddressableLED(LedConstants.kLedPort);
+    private AddressableLEDBuffer ledBuffer;
+
+    public LedSubsystem() {
+        ledBuffer = new AddressableLEDBuffer(LedConstants.kLedLength);
+        led.setLength(ledBuffer.getLength());
+        led.setData(ledBuffer);
+        led.start();
+    }
+
+    public LedState getState() {
+        return currState;
+    }
+
+    private void setState(LedState state) {
+        currState = state;
+    }
+
+    public void setColor(int r, int g, int b) {
+        setState(LedState.SOLID);
+        for (var i = 0; i < ledBuffer.getLength(); i++) {
+            ledBuffer.setRGB(i, r, g, b);
+        }
+    }
+
+    public void setColor(Color color) {
+        setState(LedState.SOLID);
+        for (var i = 0; i < ledBuffer.getLength(); i++) {
+            ledBuffer.setLED(i, color);
+        }
+    }
+
+    public void setGreen() {
+        setColor(LedConstants.kGreen);
+    }
+
+    public void setBlue() {
+        setColor(LedConstants.kBlue);
+    }
+
+    public void setFlaming() {
+        setState(LedState.FLAMING);
+    }
+
+    public void setOff() {
+        setColor(new Color());
+        currState = LedState.OFF;
+    }
+
+    @Override
+    public void periodic() {
+
+        switch (currState) {
+            case FLAMING:
+                for (var i = 0; i < ledBuffer.getLength(); i++) {
+                    ledBuffer.setRGB(i, 250, (int) (Math.random() * 185), 0);
+                }
+                break;
+            case SOLID:
+                break;
+            case OFF:
+                break;
+            default:
+                setOff();
+                break;
+        }
+    }
+
+    @Override
+    public Set<Measure> getMeasures() {
+        return Set.of(
+            new Measure("State", () -> getState().ordinal())
+        );
+    }
+
+    public enum LedState {
+        OFF,
+        SOLID,
+        FLAMING
+    }
+
+}

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

@@ -5,12 +5,14 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.MagazineConstants;
 import frc.robot.constants.RobotStateConstants;
 import frc.robot.constants.ShooterConstants;
 import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem.IntakeState;
+import frc.robot.subsystems.led.LedSubsystem;
 import frc.robot.subsystems.magazine.MagazineSubsystem;
 import frc.robot.subsystems.magazine.MagazineSubsystem.MagazineStates;
 import frc.robot.subsystems.superStructure.SuperStructure;
@@ -35,6 +37,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem {
   private MagazineSubsystem magazineSubsystem;
   private SuperStructure superStructure;
   private ClimbSubsystem climbSubsystem;
+  private LedSubsystem ledSubsystem;
 
   private RobotStates curState = RobotStates.IDLE;
   private RobotStates nextState = RobotStates.IDLE;
@@ -75,13 +78,15 @@ public RobotStateSubsystem(
       IntakeSubsystem intakeSubsystem,
       MagazineSubsystem magazineSubsystem,
       SuperStructure superStructure,
-      ClimbSubsystem climbSubsystem) {
+      ClimbSubsystem climbSubsystem,
+      LedSubsystem ledSubsystem) {
     this.visionSubsystem = visionSubsystem;
     this.driveSubsystem = driveSubsystem;
     this.intakeSubsystem = intakeSubsystem;
     this.magazineSubsystem = magazineSubsystem;
     this.superStructure = superStructure;
     this.climbSubsystem = climbSubsystem;
+    this.ledSubsystem = ledSubsystem;
     grabElbowOffsetPreferences();
     parseLookupTable();
   }
@@ -224,6 +229,7 @@ public void toIntake() {
     superStructure.intake();
     // magazineSubsystem.toIntaking();
     magazineSubsystem.setEmpty();
+    ledSubsystem.setFlaming();
     setState(RobotStates.TO_INTAKING);
   }
 
@@ -451,6 +457,7 @@ public void periodic() {
 
           intakeSubsystem.setPercent(0);
 
+          ledSubsystem.setGreen();
           toStow();
         }
         if (intakeSubsystem.getState() == IntakeState.HAS_PIECE
@@ -521,6 +528,9 @@ public void periodic() {
 
           curShot += 1;
           hasShootBeamUnbroken = false;
+          if (isAuto) {
+            ledSubsystem.setBlue();
+          }
 
           setState(RobotStates.SHOOTING);
         }
@@ -540,6 +550,7 @@ public void periodic() {
           magazineSubsystem.setSpeed(0);
 
           superStructure.stopShoot();
+          ledSubsystem.setOff();
           toIntake();
           magazineSubsystem.setEmpty();
         }
@@ -551,6 +562,9 @@ public void periodic() {
           // superStructure.slowWheelSpin();
           superStructure.stopShoot();
         }
+        if (magazineSubsystem.getSpeed() >= MagazineConstants.kPodiumRumbleSpeed) {
+          ledSubsystem.setBlue();
+        }
         // if (superStructure.isFinished() && magazineSubsystem.getState() ==
         // MagazineStates.SHOOT)
         // {
@@ -569,6 +583,7 @@ public void periodic() {
           magazineShootDelayTimer.stop();
 
           superStructure.stopPodiumShoot();
+          ledSubsystem.setOff();
           toStow();
         }
         break;