optimize super cycles

Author: mwitcpalek

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

@@ -32,6 +32,7 @@ public class AlgaeConstants {
   public static final double kIntakingSpeed = -1;
 
   public static final double kHasAlgaeVelThreshold = 10;
+  public static final double kSuperCycleHasAlgaeVelThres = 50;
   public static final double kHasAlgaeCounts = 2;
 
   // Example Talon FX Config

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

@@ -20,7 +20,7 @@
 
 public class ElevatorConstants {
 
-  public static final double kCloseEnoughRotations = 0.1;
+  public static final double kCloseEnoughRotations = 0.4;
   public static final double kStowThresholdDone = 0.3676757 + 0.5;
   public static final double kMaxFwd = 53;
   public static final double kMaxRev = 0.1;

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

@@ -12,6 +12,9 @@ public class RobotStateConstants {
 
   public static final double kProcessorStowRadius = 0.5;
 
+  // Super Cycle Constants
+  public static final double kBiscuitSuperCycleSafeThres = 6;
+
   // Elevator good for climb, tolerates stuck coral
   public static final double kElevatorClimbMax = 11.4;
 

src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java

@@ -57,6 +57,12 @@ public boolean hasAlgae() {
     return curState == AlgaeStates.HAS_ALGAE;
   }
 
+  public boolean hasAlgaeSuperCycle() {
+    return curState == AlgaeStates.HAS_ALGAE
+        || (FastMath.abs(inputs.velocity) < AlgaeConstants.kSuperCycleHasAlgaeVelThres
+            && inputs.isBeamBroken);
+  }
+
   public void setSpeed(double speed) {
     // io.setSpeed(speed);
     // desiredSpeed = speed;

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

@@ -720,15 +720,15 @@ public void periodic() {
         }
       }
       case REEF_ALIGN_ALGAE -> {
-        if (algaeSubsystem.hasAlgae()) {
+        if (algaeSubsystem.hasAlgaeSuperCycle()) {
           biscuitSubsystem.setIsRemovingAlgae(true);
           switch (getAlgaeLevel()) {
             case L2 -> {
-              biscuitSubsystem.setPosition(RobotConstants.kL2AlgaeRemovalSetpoint, hasAlgae());
+              biscuitSubsystem.setPosition(RobotConstants.kL2AlgaeRemovalSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint);
             }
             case L3 -> {
-              biscuitSubsystem.setPosition(RobotConstants.kL3AlgaeRemovalSetpoint, hasAlgae());
+              biscuitSubsystem.setPosition(RobotConstants.kL3AlgaeRemovalSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeRemovalSetpoint);
             }
             default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
@@ -750,7 +750,9 @@ public void periodic() {
         }
       }
       case REMOVE_ALGAE -> {
-        if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) {
+        if (elevatorSubsystem.isFinished()
+            && biscuitSubsystem.getPosition().in(Rotations)
+                <= RobotStateConstants.kBiscuitSuperCycleSafeThres) {
           biscuitSubsystem.setIsRemovingAlgae(false);
           if (coralSubsystem.hasCoral()
               && !(isAutoPlacing && getAlgaeOnCycle && scoreSide == ScoreSide.RIGHT)) {