Merge branch 'statesDrivePractice' of https://github.com/strykeforce/reefscape into statesDrivePractice

Author: ds12a

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

@@ -13,12 +13,15 @@ public static final Color invertRedGreen(Color color) {
   public static final int kTotalStripLength = 51 + 10;
   public static final int kBottomStripLength = 34;
   public static final int kTopStripLength = 17;
+  public static final int kFunnelStripLength = 10;
   public static final int kTopFirstIndex = kBottomStripLength;
+  public static final int kFunnelFirstIndex = kTopFirstIndex + kTopStripLength;
   public static final int kAlgeaEnd = kBottomStripLength / 3 + 1;
 
   public static final int kLevelStart = kTopFirstIndex;
   public static final int kPlaceStart = kTopFirstIndex + kTopStripLength / 3;
   public static final int kGetAlgeaStart = kTopFirstIndex + kTopStripLength / 3 * 2;
+  public static final int kAlgeaHeightStart = kFunnelFirstIndex;
   public static final int kAutoPlacingStart = kBottomStripLength / 3 * 2 + 1;
 
   public static final Color kAlmostBlack = new Color(0, 0, 1);
@@ -55,6 +58,10 @@ public static final Color invertRedGreen(Color color) {
   // HP Load Color
   public static final Color kLoadCoral = invertRedGreen(Color.kBlue);
 
+  // Algea Height Colors
+  public static final Color kAlgeaLow = invertRedGreen(Color.kSaddleBrown);
+  public static final Color kAlgeaHigh = invertRedGreen(Color.kDarkSlateBlue);
+
   public static final Color[] kGameColors = {
     invertRedGreen(Color.kBlack), // a dummy color
     invertRedGreen(Color.kDarkRed),

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

@@ -20,7 +20,7 @@ public class RobotStateConstants {
   public static final double kElevatorWaitRadius = 1.5;
 
   // Super Cycle Constants
-  public static final double kBiscuitSuperCycleSafeThres = 7; // 6 is max
+  public static final double kBiscuitSuperCycleSafeThres = 8; // 6 is max
 
   // Elevator good for climb, tolerates stuck coral
   public static final double kElevatorClimbMax = 11.4;

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

@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj.LEDPattern;
 import edu.wpi.first.wpilibj.util.Color;
 import frc.robot.constants.LEDConstants;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.AlgaeHeight;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.CoralLoc;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
 import java.util.Map;
@@ -37,6 +38,11 @@ public class LEDSubsystem extends MeasurableSubsystem {
           Map.of(
               LEDConstants.kGetAlgeaStart / (double) LEDConstants.kTotalStripLength,
               LEDConstants.kNotGetAlgea));
+  private LEDPattern algeaHeight =
+      LEDPattern.steps(
+          Map.of(
+              LEDConstants.kAlgeaHeightStart / (double) LEDConstants.kTotalStripLength,
+              LEDConstants.kAlgeaLow));
   private LEDPattern autoplace =
       LEDPattern.steps(
               Map.of(
@@ -57,6 +63,7 @@ public class LEDSubsystem extends MeasurableSubsystem {
   private ScoringLevel levelState = ScoringLevel.L4;
   private PlaceStates placeState = PlaceStates.MANUAL;
   private boolean shouldGetAlgae = false;
+  private AlgaeHeight algaeHeight = AlgaeHeight.LOW;
   private boolean shouldLoadCoral = false;
   private boolean autoPlacing = false;
   private boolean isLimiting = false;
@@ -111,6 +118,11 @@ public void setGetAlgeaLights(boolean on) {
     buildBase();
   }
 
+  public void setAlgeaHeight(AlgaeHeight algaeHeight) {
+    this.algaeHeight = algaeHeight;
+    buildBase();
+  }
+
   public void setAutoPlacing(boolean isAutoPlacing) {
     autoPlacing = isAutoPlacing;
     buildBase();
@@ -147,6 +159,10 @@ public boolean getGetAlgeaLights() {
     return shouldGetAlgae;
   }
 
+  public AlgaeHeight getIsAlgeaHigh() {
+    return algaeHeight;
+  }
+
   public boolean getAutoPlacing() {
     return autoPlacing;
   }
@@ -214,6 +230,19 @@ private void buildBase() {
               Map.of(
                   LEDConstants.kGetAlgeaStart / (double) LEDConstants.kTotalStripLength,
                   shouldGetAlgae ? LEDConstants.kGetAlgea : LEDConstants.kNotGetAlgea));
+      switch (algaeHeight) {
+        case HIGH -> algeaHeight =
+            LEDPattern.steps(
+                Map.of(
+                    LEDConstants.kAlgeaHeightStart / (double) LEDConstants.kTotalStripLength,
+                    LEDConstants.kAlgeaHigh));
+
+        case LOW -> algeaHeight =
+            LEDPattern.steps(
+                Map.of(
+                    LEDConstants.kAlgeaHeightStart / (double) LEDConstants.kTotalStripLength,
+                    LEDConstants.kAlgeaLow));
+      }
       base = algae.overlayOn(coral);
     }
 
@@ -227,7 +256,7 @@ private void buildBase() {
     if (shouldLoadCoral) {
       base = loadCoral.overlayOn(base);
     } else {
-      base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(base)));
+      base = algeaHeight.overlayOn(getAlgea.overlayOn(place.overlayOn(level.overlayOn(base))));
     }
   }
 
@@ -332,14 +361,22 @@ public Set<Measure> getMeasures() {
             "GetAlgea",
             "the current get algea state of the LEDSubsystem",
             () -> shouldGetAlgae ? 0 : 1),
+        new Measure(
+            "isAlgeaHigh",
+            "The assumed Algea Height of the LEDSubsystem",
+            () -> algaeHeight.ordinal()),
         new Measure(
             "autoPlacing",
             "the current autoPlacing state of the LEDSubsystem",
             () -> autoPlacing ? 0 : 1),
         new Measure(
             "currentLimiting",
             "the current currentLimiting state of the LEDSubsystem",
-            () -> isLimiting ? 0 : 1));
+            () -> isLimiting ? 0 : 1),
+        new Measure(
+            "isHPLoading",
+            "the current HP Loading state of the LEDSubsystem",
+            () -> shouldLoadCoral ? 0 : 1));
   }
 
   public enum LEDStates {

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

@@ -238,10 +238,12 @@ public void setScoreSide(ScoreSide scoreSide) {
 
   public void setAlgaeHeight(AlgaeHeight algaeHeight) {
     this.algaeHeight = algaeHeight;
+    ledSubsystem.setAlgeaHeight(algaeHeight);
   }
 
   public void toggleAlgaeHeight() {
     algaeHeight = algaeHeight == AlgaeHeight.LOW ? AlgaeHeight.HIGH : AlgaeHeight.LOW;
+    ledSubsystem.setAlgeaHeight(algaeHeight);
   }
 
   public void setIsAutoPlacing(boolean isAutoPlacing) {