underinflated algae, stop tag align properly, safe prestage sequence

Author: ds12a

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

@@ -119,7 +119,9 @@ public static TalonFXConfiguration getBothFXConfig() {
     fxConfig.Slot0 = slot0;
 
     MotionMagicConfigs motionMagic =
-        new MotionMagicConfigs().withMotionMagicCruiseVelocity(75).withMotionMagicAcceleration(300);
+        new MotionMagicConfigs()
+            .withMotionMagicCruiseVelocity(75)
+            .withMotionMagicAcceleration(225); // was 300
     fxConfig.MotionMagic = motionMagic;
 
     MotorOutputConfigs motorOut =

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

@@ -350,8 +350,8 @@ public static class CompConstants {
     public static Angle kPrestageAlgaeSetpoint = kBiscuitStowSetpoint;
 
     // Algae removal
-    public static Angle kL2AlgaeSetpoint = Rotations.of(7.39);
-    public static Angle kL3AlgaeSetpoint = Rotations.of(7.39);
+    public static Angle kL2AlgaeSetpoint = Rotations.of(8.125); // was 7.39
+    public static Angle kL3AlgaeSetpoint = Rotations.of(8.125); // was 7.39
 
     public static Angle kL2AlgaeRemovalSetpoint = kPrestageSetpoint;
     public static Angle kL3AlgaeRemovalSetpoint = kPrestageSetpoint;

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

@@ -242,8 +242,9 @@ public void toggleGetAlgaeOnCycle() {
     ledSubsystem.setGetAlgeaLights(getAlgaeOnCycle);
   }
 
-  public void setBiscuitTransfer(Angle setpoint) {
-    if (elevatorSubsystem.getPosition().gt(ElevatorConstants.kBiscuitSafeThreshold)) {
+  public void setBiscuitTransfer(Angle setpoint, boolean overrideThreshold) {
+    if (overrideThreshold
+        || elevatorSubsystem.getPosition().gt(ElevatorConstants.kBiscuitSafeThreshold)) {
       biscuitSubsystem.setPosition(setpoint, hasAlgae());
     }
     nextBiscuitSetpoint = setpoint;
@@ -310,7 +311,7 @@ public void toStow() {
     driveSubsystem.setIgnoreSticks(false);
 
     if (biscuitSubsystem.isSafeToStow()) {
-      setBiscuitTransfer(RobotConstants.kStowSetpoint);
+      setBiscuitTransfer(RobotConstants.kStowSetpoint, true);
       elevatorSubsystem.setPosition(RobotConstants.kElevatorStowSetpoint);
       algaeSubsystem.hold();
 
@@ -321,7 +322,7 @@ public void toStow() {
   }
 
   public void toStowSafe() {
-    setBiscuitTransfer(RobotConstants.kStowSetpoint);
+    setBiscuitTransfer(RobotConstants.kStowSetpoint, true);
     driveSubsystem.removeDriveMultiplier();
     driveSubsystem.setIgnoreSticks(false);
     algaeSubsystem.hold();
@@ -330,7 +331,7 @@ public void toStowSafe() {
   }
 
   public void toStowSequential() {
-    setBiscuitTransfer(RobotConstants.kStowSetpoint);
+    setBiscuitTransfer(RobotConstants.kStowSetpoint, true);
     driveSubsystem.removeDriveMultiplier();
     driveSubsystem.setIgnoreSticks(false);
     algaeSubsystem.hold();
@@ -340,23 +341,23 @@ public void toStowSequential() {
 
   public void toAutonPrestage() {
     coralLoc = CoralLoc.CORAL;
-    setBiscuitTransfer(RobotConstants.kPrestageSetpoint);
+    setBiscuitTransfer(RobotConstants.kPrestageSetpoint, false);
     elevatorSubsystem.setPosition(ElevatorConstants.kAutoPrestageSetpoint);
     funnelSubsystem.stopMotor();
 
     setState(RobotStates.PRESTAGE, true);
   }
 
   public void toFunnelLoad() {
-    setBiscuitTransfer(RobotConstants.kFunnelSetpoint);
+    setBiscuitTransfer(RobotConstants.kFunnelSetpoint, true);
     coralSubsystem.intake();
     elevatorSubsystem.setPosition(RobotConstants.kElevatorFunnelSetpoint);
 
     setState(RobotStates.FUNNEL_LOAD, true);
   }
 
   private void toPrestage() {
-    setBiscuitTransfer(RobotConstants.kPrestageSetpoint);
+    setBiscuitTransfer(RobotConstants.kPrestageSetpoint, false);
 
     prestagingForAlgae = getAlgaeOnCycle;
 
@@ -424,7 +425,7 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
 
     if (wantAlgae && !algaeSafe) {
       algaeSubsystem.intake();
-      setBiscuitTransfer(RobotConstants.kPrestageAlgaeSetpoint);
+      setBiscuitTransfer(RobotConstants.kPrestageAlgaeSetpoint, true);
 
       switch (getAlgaeLevel()) {
         case L2 -> {
@@ -451,11 +452,11 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
 
       switch (getAlgaeLevel()) {
         case L2 -> {
-          setBiscuitTransfer(RobotConstants.kL2AlgaeSetpoint);
+          setBiscuitTransfer(RobotConstants.kL2AlgaeSetpoint, true);
           elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeSetpoint);
         }
         case L3 -> {
-          setBiscuitTransfer(RobotConstants.kL3AlgaeSetpoint);
+          setBiscuitTransfer(RobotConstants.kL3AlgaeSetpoint, true);
           elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeSetpoint);
         }
         default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
@@ -476,19 +477,19 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
           currentLevel = scoringLevel;
           switch (scoringLevel) {
             case L1 -> {
-              setBiscuitTransfer(RobotConstants.kL1CoralSetpoint);
+              setBiscuitTransfer(RobotConstants.kL1CoralSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint);
             }
             case L2 -> {
-              setBiscuitTransfer(RobotConstants.kL2CoralSetpoint);
+              setBiscuitTransfer(RobotConstants.kL2CoralSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint);
             }
             case L3 -> {
-              setBiscuitTransfer(RobotConstants.kL3CoralSetpoint);
+              setBiscuitTransfer(RobotConstants.kL3CoralSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint);
             }
             case L4 -> {
-              setBiscuitTransfer(RobotConstants.kL4CoralSetpoint);
+              setBiscuitTransfer(RobotConstants.kL4CoralSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint);
             }
           }
@@ -535,7 +536,7 @@ public void toAlgaeFloorPickup() {
           return;
         }
 
-        setBiscuitTransfer(RobotConstants.kFloorAlgaeSetpoint);
+        setBiscuitTransfer(RobotConstants.kFloorAlgaeSetpoint, true);
         elevatorSubsystem.setPosition(ElevatorConstants.kFloorAlgaeSetpoint);
 
         setState(RobotStates.FLOOR_ALGAE, true);
@@ -545,7 +546,7 @@ public void toAlgaeFloorPickup() {
           return;
         }
 
-        setBiscuitTransfer(RobotConstants.kMicAlgaeSetpoint);
+        setBiscuitTransfer(RobotConstants.kMicAlgaeSetpoint, true);
         elevatorSubsystem.setPosition(ElevatorConstants.kMicAlgaeSetpoint);
 
         setState(RobotStates.MIC_ALGAE, true);
@@ -633,7 +634,7 @@ public void toHpAlgae() {
 
     algaeSubsystem.intake();
 
-    setBiscuitTransfer(RobotConstants.kHpAlgaeSetpoint);
+    setBiscuitTransfer(RobotConstants.kHpAlgaeSetpoint, false);
     elevatorSubsystem.setPosition(ElevatorConstants.kHpAlgaeSetpoint);
 
     setState(RobotStates.HP_ALGAE, true);
@@ -650,7 +651,7 @@ private void toProcessor() {
       return;
     }
 
-    setBiscuitTransfer(RobotConstants.kProcessorSetpoint);
+    setBiscuitTransfer(RobotConstants.kProcessorSetpoint, true);
     elevatorSubsystem.setPosition(ElevatorConstants.kProcessorSetpoint);
 
     setState(RobotStates.PROCESSOR_ALGAE, true);
@@ -671,7 +672,7 @@ public void toInterrupted() {
 
     biscuitSubsystem.setIsRemovingAlgae(false);
 
-    setBiscuitTransfer(biscuitSubsystem.getPosition());
+    biscuitSubsystem.setPosition(biscuitSubsystem.getPosition(), hasAlgae());
     elevatorSubsystem.setPosition(elevatorSubsystem.getPosition());
 
     setState(RobotStates.INTERRUPTED);
@@ -682,7 +683,7 @@ public void toPrepClimb() {
       return;
     }
 
-    setBiscuitTransfer(RobotConstants.kStowSetpoint);
+    setBiscuitTransfer(RobotConstants.kStowSetpoint, true);
     elevatorSubsystem.setPosition(RobotConstants.kElevatorStowSetpoint);
 
     climbSubsystem.prepClimb();

src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java

@@ -434,7 +434,7 @@ public void periodic() {
                 currentThresCount++;
                 if (currentThresCount >= TagServoingConstants.kEndCountThreshold) {
                   terminate();
-                  break;
+                  return;
                 }
               } else {
                 currentThresCount = 0;