Add getter command for ALGAE height

	modified:   src/main/deploy/pathplanner/settings.json
	modified:   src/main/java/frc/robot/Constants.java
	modified:   src/main/java/frc/robot/RobotContainer.java
	modified:   src/main/java/frc/robot/subsystems/state_keeper/ReefTarget.java

src/main/deploy/pathplanner/settings.json

@@ -45,4 +45,4 @@
     "{\"name\":\"Ground Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.34,\"y\":0.0},\"size\":{\"width\":0.8,\"length\":0.1},\"borderRadius\":0.02,\"strokeWidth\":0.01,\"filled\":false}}",
     "{\"name\":\"Circle\",\"type\":\"circle\",\"data\":{\"center\":{\"x\":-0.46,\"y\":0.0},\"radius\":0.032,\"strokeWidth\":0.01,\"filled\":true}}"
   ]
-}
+}

src/main/java/frc/robot/Constants.java

@@ -192,8 +192,8 @@ public static final class ElevatorConstants {
     public static final Distance kL2 = Inches.of(48.125);
     public static final Distance kL3 = Inches.of(56.25);
     public static final Distance kL4 = Inches.of(68.125);
-    public static final Distance KAlgae1 = Inches.of(58.375);
-    public static final Distance KAlgae2 = Inches.of(65.75);
+    public static final Distance KAlgaeLower = Inches.of(58.375);
+    public static final Distance KAlgaeUpper = Inches.of(65.75);
     // Motion Magic constants
     public static final LinearVelocity kVelocity = MetersPerSecond.of(1.8);
     public static final LinearAcceleration kAcceleration = MetersPerSecondPerSecond.of(2.8);

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

@@ -265,16 +265,18 @@ public RobotContainer() {
                 .withTimeout(1)
                 .andThen(Commands.run(() -> m_coralScorer.setCoralPercent(.70), m_coralScorer))));
     NamedCommands.registerCommand("L3", Commands.print("L3")); // Just print commands for right now.
-    NamedCommands.registerCommand("L2",         Commands.parallel( // Needs to be canceled with a race group right now, the race group wait
-    // timer is at 0.8 seconds.
-    new ElevatorCommand(
-        () -> ElevatorConstants.kL2, // Change this to kL4 or kL3 for those levels
-        ElevatorConstants.kAcceleration,
-        ElevatorConstants.kVelocity,
-        m_elevator),
-    Commands.run(() -> m_coralScorer.setCoralPercent(.0), m_coralScorer)
-        .withTimeout(0.4)
-        .andThen(Commands.run(() -> m_coralScorer.setCoralPercent(.70), m_coralScorer))));
+    NamedCommands.registerCommand(
+        "L2",
+        Commands.parallel( // Needs to be canceled with a race group right now, the race group wait
+            // timer is at 0.8 seconds.
+            new ElevatorCommand(
+                () -> ElevatorConstants.kL2, // Change this to kL4 or kL3 for those levels
+                ElevatorConstants.kAcceleration,
+                ElevatorConstants.kVelocity,
+                m_elevator),
+            Commands.run(() -> m_coralScorer.setCoralPercent(.0), m_coralScorer)
+                .withTimeout(0.4)
+                .andThen(Commands.run(() -> m_coralScorer.setCoralPercent(.70), m_coralScorer))));
 
     NamedCommands
         .registerCommand( // Brings the elevator to the ground. Put after the race group to score.

src/main/java/frc/robot/subsystems/state_keeper/ReefTarget.java

@@ -57,7 +57,6 @@ private ReefTarget() {}
 
   /** Periodic function includes logging and publishing to NT */
   public synchronized void periodic() {
-    getElevatorDelay();
     // Log to AdvantageKit
     Logger.recordOutput("ReefTarget/Post_ALL", convertIntToAlphabet(reefPostAll + 1));
     Logger.recordOutput("ReefTarget/Post_LR", reefPostLR);
@@ -162,16 +161,20 @@ public Distance getElevatorHeight() {
     }
   }
 
-  public void getElevatorDelay() {
-
-    if (reefLevel == 2) {
-      elevatorDelay = .35;
-    } else if (reefLevel == 3) {
-      elevatorDelay = .55;
-    } else if (reefLevel == 4) {
-      elevatorDelay = .85;
-    } else {
-      elevatorDelay = .35;
+  /**
+   * Return the elevator height needed to access the ALGAE based on the currently selected REEF face
+   */
+  public Distance getElevatorAlgae() {
+    switch ((reefPostAll / 2) % 2) {
+      case 0:
+        // Reef Face A/B, E/F, I/J -- ALGAE between L3 and L4
+        return ElevatorConstants.KAlgaeUpper;
+      case 1:
+        // Reef Face C/D, G/H, K/L -- ALGAE between L2 and L3
+        return ElevatorConstants.KAlgaeLower;
+      default:
+        // Shouldn't run, but you know...
+        return ElevatorConstants.kElevatorZeroHeight;
     }
   }