Algae removal optimize

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,10 +20,10 @@
 
 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 = 2;
+  public static final double kMaxRev = 0.1;
   public static final double kElevatorLiftHeight = 1; // for pit command
   public static final int kZeroMultiple =
       0; // some constant to multiply, add by to turn the analog input into a position
@@ -76,7 +76,7 @@ public static TalonFXConfiguration getBothFXConfig() {
         new CurrentLimitsConfigs()
             .withStatorCurrentLimitEnable(false)
             .withSupplyCurrentLimitEnable(true)
-            .withSupplyCurrentLimit(40)
+            .withSupplyCurrentLimit(70)
             .withSupplyCurrentLowerLimit(10)
             .withSupplyCurrentLowerTime(2);
     fxConfig.CurrentLimits = current;
@@ -114,7 +114,7 @@ public static TalonFXConfiguration getBothFXConfig() {
     fxConfig.Slot0 = slot0;
 
     MotionMagicConfigs motionMagic =
-        new MotionMagicConfigs().withMotionMagicCruiseVelocity(50).withMotionMagicAcceleration(100);
+        new MotionMagicConfigs().withMotionMagicCruiseVelocity(75).withMotionMagicAcceleration(300);
     fxConfig.MotionMagic = motionMagic;
 
     MotorOutputConfigs motorOut =

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

@@ -39,8 +39,9 @@ public class RobotConstants {
 
   // Biscuit
   public static TalonFXSConfiguration talonFXSConfig;
-  public static MotionMagicConfigs alageMotionConfig;
-  public static MotionMagicConfigs noAlageMotionConfig;
+  public static MotionMagicConfigs algaeMotionConfig;
+  public static MotionMagicConfigs algaeRemovalMotionConfig;
+  public static MotionMagicConfigs noAlgaeMotionConfig;
 
   public static double kTicksPerRot;
 
@@ -92,8 +93,9 @@ public RobotConstants() {
       kElevatorFunnelSetpoint = CompConstants.kElevatorFunnelSetpoint;
       kElevatorStowSetpoint = CompConstants.kElevatorStowSetpoint;
       talonFXSConfig = CompConstants.getFXSConfig();
-      alageMotionConfig = CompConstants.getAlgaeMotionConfig();
-      noAlageMotionConfig = CompConstants.getNoAlgaeMotionConfig();
+      algaeMotionConfig = CompConstants.getAlgaeMotionConfig();
+      algaeRemovalMotionConfig = CompConstants.getAlgaeRemovalMotionConfig();
+      noAlgaeMotionConfig = CompConstants.getNoAlgaeMotionConfig();
       kTicksPerRot = 80;
       logger.info("Using Comp Constants");
 
@@ -138,8 +140,9 @@ public RobotConstants() {
       kElevatorFunnelSetpoint = ProtoConstants.kElevatorFunnelSetpoint;
       kElevatorStowSetpoint = ProtoConstants.kElevatorStowSetpoint;
       talonFXSConfig = ProtoConstants.getFXSConfig();
-      alageMotionConfig = ProtoConstants.getAlgaeMotionConfig();
-      noAlageMotionConfig = ProtoConstants.getNoAlgaeMotionConfig();
+      algaeMotionConfig = ProtoConstants.getAlgaeMotionConfig();
+      algaeRemovalMotionConfig = algaeMotionConfig;
+      noAlgaeMotionConfig = ProtoConstants.getNoAlgaeMotionConfig();
       kTicksPerRot = 160;
       logger.info("Using Proto Constants");
 
@@ -385,6 +388,17 @@ public static MotionMagicConfigs getAlgaeMotionConfig() {
       return algaeConfig;
     }
 
+    public static MotionMagicConfigs getAlgaeRemovalMotionConfig() {
+      MotionMagicConfigs algaeConfig =
+          new MotionMagicConfigs()
+              .withMotionMagicAcceleration(300)
+              .withMotionMagicCruiseVelocity(30)
+              .withMotionMagicExpo_kA(0)
+              .withMotionMagicExpo_kV(0)
+              .withMotionMagicJerk(1500);
+      return algaeConfig;
+    }
+
     public static MotionMagicConfigs getNoAlgaeMotionConfig() {
       MotionMagicConfigs algaeConfig =
           new MotionMagicConfigs()

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/biscuit/BiscuitIO.java

@@ -17,6 +17,8 @@ public class BiscuitIOInputs {
 
   public default void setPosition(Angle position, boolean hasAlgae) {}
 
+  public default void setIsRemovingAlgae(boolean isRemoving) {}
+
   public default void updateInputs(BiscuitIOInputs inputs) {}
 
   public default void registerWith(TelemetryService telemetry) {}

src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFXS.java

@@ -47,6 +47,7 @@ public class BiscuitIOFXS implements BiscuitIO, Checkable {
   private Angle offset;
   private Alert rangeAlert = new Alert("Biscuit overextended! Shuting down!", AlertType.kError);
   private boolean lastHadAlgae = false;
+  private boolean isRemovingAlgae = false;
 
   private TalonFXSConfigurator configurator;
   private MotionMagicDutyCycle positionRequest =
@@ -75,13 +76,22 @@ public BiscuitIOFXS() {
     zero();
   }
 
+  @Override
+  public void setIsRemovingAlgae(boolean isRemoving) {
+    this.isRemovingAlgae = isRemoving;
+  }
+
   @Override
   public void setPosition(Angle position, boolean hasAlgae) {
     if (hasAlgae != lastHadAlgae) {
       if (hasAlgae) {
-        configurator.apply(RobotConstants.alageMotionConfig);
+        if (isRemovingAlgae) {
+          configurator.apply(RobotConstants.algaeMotionConfig);
+        } else {
+          configurator.apply(RobotConstants.algaeMotionConfig);
+        }
       } else {
-        configurator.apply(RobotConstants.noAlageMotionConfig);
+        configurator.apply(RobotConstants.noAlgaeMotionConfig);
       }
       lastHadAlgae = hasAlgae;
     }
@@ -90,12 +100,11 @@ public void setPosition(Angle position, boolean hasAlgae) {
 
   @Override
   public void updateInputs(BiscuitIOInputs inputs) {
+    BaseStatusSignal.refreshAll(velocity, position, rawPulseWidth);
     inputs.velocity = velocity.getValueAsDouble();
     inputs.position = position.getValueAsDouble();
     inputs.rawPulseWidth = rawPulseWidth.getValueAsDouble();
     inputs.didZero = didZero;
-
-    BaseStatusSignal.refreshAll(velocity, position, rawPulseWidth);
   }
 
   @Override

src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java

@@ -34,6 +34,10 @@ public boolean hasZeroed() {
     return hasZeroed;
   }
 
+  public void setIsRemovingAlgae(boolean isRemoving) {
+    io.setIsRemovingAlgae(isRemoving);
+  }
+
   public void setPosition(Angle position, boolean hasAlgae) {
     io.setPosition(position, hasAlgae);
     setPoint = position;

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

@@ -290,6 +290,8 @@ private boolean needSafeAlgaeTransfer(RobotStates nextState) {
 
   public void toStow() {
     visionSubsystem.setYawUpdateCamera(-1);
+    biscuitSubsystem.setIsRemovingAlgae(false);
+
     if (biscuitSubsystem.isSafeToStow()) {
       biscuitSubsystem.setPosition(RobotConstants.kStowSetpoint, hasAlgae());
       elevatorSubsystem.setPosition(RobotConstants.kElevatorStowSetpoint);
@@ -609,6 +611,8 @@ public void toInterrupted() {
       driveSubsystem.setIgnoreSticks(false);
     }
 
+    biscuitSubsystem.setIsRemovingAlgae(false);
+
     biscuitSubsystem.setPosition(biscuitSubsystem.getPosition(), hasAlgae());
     elevatorSubsystem.setPosition(elevatorSubsystem.getPosition());
 
@@ -716,17 +720,15 @@ public void periodic() {
         }
       }
       case REEF_ALIGN_ALGAE -> {
-        if (biscuitSubsystem.getPosition().in(Rotations) > RobotConstants.kTagAlignThreshold) {
-          tagAlignSubsystem.setProceedToAlign(true);
-        }
-        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());
@@ -748,7 +750,10 @@ 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)) {
             toReefAlign(false, false);

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

@@ -75,7 +75,7 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu
 
     Logger.recordOutput("TagAlignSubsystem/Hexant", -1);
 
-    driveRadius = 1.223823;
+    driveRadius = 1.293823;
     for (int i = 0; i < 6; i++) {
       logger.info("Hexant {}, left and right", i);