add LED subsystem calls to robotState

mwitcpalek · mwitcpalek · commit 98a9de0ca940 · 2025-02-23T14:31:51.000-05:00
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -135,7 +135,7 @@ public RobotContainer() {
     funnelSubsystem = new FunnelSubsystem(funnelIO);
 
     ledIO = new LEDIO();
-    ledSubsystem = new LEDSubsystem();
+    ledSubsystem = new LEDSubsystem(ledIO);
 
     visionSubsystem = new VisionSubsystem(driveSubsystem);
 
diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java
@@ -3,11 +3,12 @@
 import edu.wpi.first.wpilibj.util.Color;
 
 public class LEDConstants {
+  public static final int kLEDPort = 0;
   // Auto/Operator = 27
   // other = 21.5
-  public static final int kTotalStripLength = 70;
-  public static final int kBottomStripLength = 43;
-  public static final int kTopStripLength = 27;
+  public static final int kTotalStripLength = 51;
+  public static final int kBottomStripLength = 34;
+  public static final int kTopStripLength = 17;
   public static final int kTopFirstIndex = kBottomStripLength + 1;
 
   public static final int kAlgeaEnd = kBottomStripLength / 3 + 1;
diff --git a/src/main/java/frc/robot/subsystems/led/LEDIO.java b/src/main/java/frc/robot/subsystems/led/LEDIO.java
@@ -12,11 +12,11 @@ public class LEDIO {
   private AddressableLEDBuffer led;
   private AddressableLEDBufferView ledTop;
 
-  public LEDIO(int port, int length) {
-    ledBase = new AddressableLED(port);
-    ledBase.setLength(length);
+  public LEDIO() {
+    ledBase = new AddressableLED(LEDConstants.kLEDPort);
+    ledBase.setLength(LEDConstants.kTotalStripLength);
     ledBase.start();
-    led = new AddressableLEDBuffer(length);
+    led = new AddressableLEDBuffer(LEDConstants.kTotalStripLength);
     ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength + 1);
     ledBase.setData(led);
   }
diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
@@ -1,15 +1,12 @@
 package frc.robot.subsystems.led;
 
-import static edu.wpi.first.units.Units.Percent;
 import static edu.wpi.first.units.Units.Seconds;
 
 import edu.wpi.first.wpilibj.LEDPattern;
-import edu.wpi.first.wpilibj.LEDPattern.GradientType;
 import edu.wpi.first.wpilibj.util.Color;
 import frc.robot.constants.LEDConstants;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.CoralLoc;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
-
 import java.util.Map;
 import java.util.Set;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
@@ -22,8 +19,7 @@ public class LEDSubsystem extends MeasurableSubsystem {
 
   // section patterns
   private LEDPattern algea =
-      LEDPattern.steps(
-          Map.of(0, LEDConstants.kHasAlgea, LEDConstants.kAlgeaEnd, Color.kBlack));
+      LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.kAlgeaEnd, Color.kBlack));
   private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot);
   private LEDPattern level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL1));
   private LEDPattern place =
@@ -196,15 +192,13 @@ private void buildBase() {
     base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(algea.overlayOn(coral))));
   }
 
-  //game stuff
+  // game stuff
   private void advanceUnits() {
-    for (int i = 0; i <= LEDConstants.kTopStripLength+1; i++) {
+    for (int i = 0; i <= LEDConstants.kTopStripLength + 1; i++) {
       BottomUnits[i + 1] = BottomUnits[i];
       TopUnits[i] = TopUnits[i + 1];
-      if (i == 0) 
-        BottomUnits[i] = 0;
-      if (i == LEDConstants.kTopStripLength)
-        TopUnits[i] = 0;
+      if (i == 0) BottomUnits[i] = 0;
+      if (i == LEDConstants.kTopStripLength) TopUnits[i] = 0;
       if (TopUnits[i] != 0 && BottomUnits[i] != 0) {
         if (BottomUnits[i] == TopUnits[i]) {
           BottomUnits[i] = 0;
@@ -219,7 +213,8 @@ private void advanceUnits() {
         if (BottomUnits[i] == TopUnits[i + 1]) {
           BottomUnits[i] = 0;
           TopUnits[i + 1] = 0;
-        } else if (BottomUnits[i] > TopUnits[i + 1] || (BottomUnits[i] == 1 && TopUnits[i + 1] == 3)) {
+        } else if (BottomUnits[i] > TopUnits[i + 1]
+            || (BottomUnits[i] == 1 && TopUnits[i + 1] == 3)) {
           TopUnits[i + 1] = 0;
         } else {
           BottomUnits[i + 1] = 0;
@@ -229,9 +224,9 @@ private void advanceUnits() {
   }
 
   private void displayUnits() {
-    for (int i = 0; i <= LEDConstants.kTopStripLength+1; i++) {
+    for (int i = 0; i <= LEDConstants.kTopStripLength + 1; i++) {
       io.setLEDTop(i, LEDConstants.kGameColors[BottomUnits[i]]);
-      if (TopUnits[i] != 0){
+      if (TopUnits[i] != 0) {
         io.setLEDTop(i, LEDConstants.kGameColors[TopUnits[i] + 3]);
       }
     }
@@ -240,7 +235,7 @@ private void displayUnits() {
   public void addGameUnit(boolean isBottom, int unitNum) {
     // the unitNum starts at 1 and ends at 3. if you use anything else, it will be ignored
     if (unitNum >= 1 && unitNum <= 3) {
-      if (isBottom){
+      if (isBottom) {
         BottomUnits[0] = unitNum;
       } else {
         TopUnits[LEDConstants.kTopStripLength - 1] = unitNum;
diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java
@@ -17,6 +17,7 @@
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.funnel.FunnelSubsystem;
 import frc.robot.subsystems.led.LEDSubsystem;
+import frc.robot.subsystems.led.LEDSubsystem.PlaceStates;
 import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
 import frc.robot.subsystems.vision.VisionSubsystem;
 import java.util.Set;
@@ -163,10 +164,14 @@ public void setAllianceColor(Alliance alliance) {
 
   public void setScoringLevel(ScoringLevel scoringLevel) {
     this.scoringLevel = scoringLevel;
+    ledSubsystem.setLevelLights(scoringLevel);
   }
 
   public void setScoreSide(ScoreSide scoreSide) {
     this.scoreSide = scoreSide;
+    if (!isAutoPlacing) ledSubsystem.setPlaceLights(PlaceStates.MANUAL);
+    else if (scoreSide == ScoreSide.LEFT) ledSubsystem.setPlaceLights(PlaceStates.LEFT);
+    else ledSubsystem.setPlaceLights(PlaceStates.RIGHT);
   }
 
   public void setAlgaeHeight(AlgaeHeight algaeHeight) {
@@ -179,18 +184,24 @@ public void toggleAlgaeHeight() {
 
   public void setIsAutoPlacing(boolean isAutoPlacing) {
     this.isAutoPlacing = isAutoPlacing;
+    if (!isAutoPlacing) ledSubsystem.setPlaceLights(PlaceStates.MANUAL);
+    else if (scoreSide == ScoreSide.LEFT) ledSubsystem.setPlaceLights(PlaceStates.LEFT);
+    else ledSubsystem.setPlaceLights(PlaceStates.RIGHT);
   }
 
   public void setGetAlgaeOnCycle(boolean getAlgaeOnCycle) {
     this.getAlgaeOnCycle = getAlgaeOnCycle;
+    ledSubsystem.setGetAlgeaLights(getAlgaeOnCycle);
   }
 
   public void toggleGetAlgaeOnCycle() {
     getAlgaeOnCycle = !getAlgaeOnCycle;
+    ledSubsystem.setGetAlgeaLights(getAlgaeOnCycle);
   }
 
   public void setCurrentLimiting(boolean isCurrentLimiting) {
     this.isCurrentLimiting = isCurrentLimiting;
+    ledSubsystem.setCurrentLimiting(isCurrentLimiting);
   }
 
   public void setIsAuto(boolean isAuto) {
@@ -298,6 +309,7 @@ public void toReefAlign() {
   private void toReefAlign(boolean getAlgae, boolean drive) {
     if (drive) {
       tagAlignSubsystem.start(allianceColor, scoreSide == ScoreSide.LEFT);
+      setAutoPlacingLed(true);
       setState(RobotStates.REEF_ALIGN);
     }
     if ((getAlgae || !coralSubsystem.hasCoral())
@@ -473,6 +485,7 @@ private void toProcessor() {
   public void toInterrupted() {
     if (tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DONE) {
       tagAlignSubsystem.terminate();
+      setAutoPlacingLed(false);
       driveSubsystem.setIgnoreSticks(false);
     }
 
@@ -609,6 +622,7 @@ public void periodic() {
           if (coralSubsystem.hasCoral()) {
             toReefAlign(false, false);
           } else {
+            setAutoPlacingLed(false);
             toStowSequential();
           }
         }
@@ -618,6 +632,7 @@ public void periodic() {
         if (!coralSubsystem.hasCoral()
             && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) {
           coralLoc = CoralLoc.NONE;
+          setAutoPlacingLed(false);
           toFunnelLoad();
         } else {
           coralLoc = CoralLoc.SCORING;
@@ -708,6 +723,8 @@ public void periodic() {
       case IDLE -> {}
       default -> logger.error("Unhandled state: {}", curState);
     }
+    ledSubsystem.setCoralLights(coralLoc);
+    ledSubsystem.setAlgeaLights(hasAlgae());
   }
 
   @Override
