LEDs

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

@@ -22,6 +22,7 @@
 import frc.subsytem.Elevator.Elevator;
 import frc.subsytem.Elevator.ElevatorIO;
 import frc.subsytem.Elevator.ElevatorIOSparkMax;
+import frc.subsytem.LED.LED;
 import frc.subsytem.MechanismStateManager;
 import frc.subsytem.MechanismStateManager.MechanismStates;
 import frc.subsytem.drive.Drive;
@@ -86,7 +87,7 @@ public class Robot extends LoggedRobot {
     private @NotNull static TelescopingArm telescopingArm;
     private @NotNull static Grabber grabber;
     private @NotNull static MechanismStateManager mechanismStateManager;
-
+    private @NotNull static LED led;
 
     private @NotNull Controller xbox;
     private @NotNull Controller stick;
@@ -163,6 +164,7 @@ public void robotInit() {
             elevator = new Elevator(new ElevatorIOSparkMax());
             telescopingArm = new TelescopingArm(new TelescopingArmIOSparkMax());
             grabber = new Grabber(new GrabberIOSparkMax());
+            led = new LED(5);
         } else {
             setUseTiming(false); // Run as fast as possible
             Logger.getInstance().setReplaySource(new WPILOGReader(logPath)); // Read replay log
@@ -173,6 +175,7 @@ public void robotInit() {
             elevator = new Elevator(new ElevatorIO() {});
             telescopingArm = new TelescopingArm(new TelescopingArmIO() {});
             grabber = new Grabber(new GrabberIO() {});
+            led = new LED(5);
         }
 
         Logger.getInstance().start(); // Start logging! No more data receivers, replay sources, or metadata values may be added
@@ -189,6 +192,7 @@ public void robotInit() {
         telescopingArm.start();
         grabber.start();
         drive.start();
+        led.start();
         ScoringPositionManager.getInstance();
 
         xbox = new Controller(0);

src/main/java/frc/subsytem/LED/LED.java

@@ -3,18 +3,18 @@
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import frc.robot.Constants;
+import frc.robot.ScoringPositionManager;
 import frc.subsytem.AbstractSubsystem;
 import org.littletonrobotics.junction.Logger;
 
 public class LED extends AbstractSubsystem {
     AddressableLED ledStrip;
     AddressableLEDBuffer addressableLEDBuffer;
 
-    LED(int loggingInterval) {
+    public LED(int loggingInterval) {
         ledStrip = new AddressableLED(0);
         ledStrip.setLength(Constants.LED_LENGTH);
         addressableLEDBuffer = new AddressableLEDBuffer(Constants.LED_LENGTH);
-        ledStrip.setData(addressableLEDBuffer);
         ledStrip.start();
     }
 
@@ -34,16 +34,32 @@ public enum LedState {
     }
 
     public void setColor(LedState ledState) {
-        for(int i = 0; i < addressableLEDBuffer.getLength(); i++) {
+        for (int i = 0; i < addressableLEDBuffer.getLength(); i++) {
             addressableLEDBuffer.setRGB(i, ledState.r, ledState.g, ledState.b);
+            ledStrip.setData(addressableLEDBuffer);
+        }
+    }
+
+    public void setLedState() {
+        ScoringPositionManager.PositionType wantedPositionType = ScoringPositionManager.getInstance().getWantedPositionType();
+        if (wantedPositionType == ScoringPositionManager.PositionType.CONE) {
+            setColor(LedState.YELLOW);
+        } else if (wantedPositionType == ScoringPositionManager.PositionType.CUBE) {
+            setColor(LedState.BLUE);
         }
     }
 
     public void off() {
         ledStrip.close();
     }
 
+    @Override
     public void logData() {
         Logger.getInstance().recordOutput("LED Color", String.valueOf(addressableLEDBuffer.getLED(0)));
     }
+
+    @Override
+    public void update() {
+        setLedState();
+    }
 }