Merge pull request #62 from strykeforce/west-mi-hotfix

West Michigan Hotfix

Author: mwitcpalek

src/main/java/frc/robot/commands/auton/DriveAutonServoCommand.java

@@ -175,7 +175,7 @@ public void execute() {
       robotStateSubsystem.toAutonPrestage();
     }
     if (tagAlignSubsystem.getCurRadius(robotStateSubsystem.getAllianceColor())
-            <= AutonConstants.kElevatorStageRadius
+            <= AutonConstants.kElevatorStageRadiusPathOne
         && !hasPreppedCoral) {
       hasPreppedCoral = true;
       robotStateSubsystem.toPrepCoral();

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

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

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

@@ -11,6 +11,7 @@ public class AutonConstants {
   public static final double kMaxOmegaErrorRadians = Units.degreesToRadians(kMaxOmegaErrorDegrees);
   public static final int kSwitchStableCounts = 3;
   public static final double kElevatorStageRadius = 2.2;
+  public static final double kElevatorStageRadiusPathOne = 1.8;
   public static final double kInitPathPrestageTime = 0.6;
 
   // Start Poses

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

@@ -55,20 +55,25 @@ public class ElevatorConstants {
       Rotations.of(31.0901); // was 30.42969 -> 31.7505 -> 31.0901
   public static final Angle kL4CoralSetpoint = Rotations.of(48.28076);
 
-  public static final Angle kPrestageSetpoint = kL2CoralSetpoint;
   public static final Angle kAutoPrestageSetpoint = kL2CoralSetpoint;
 
   // Algae obtaining
   public static final Angle kFloorAlgaeSetpoint = Rotations.of(5.82);
   public static final Angle kMicAlgaeSetpoint = Rotations.of(2.703);
   public static final Angle kHpAlgaeSetpoint = Rotations.of(14.9063);
 
+  // pre-stage tele
+  public static final Angle kPrestageSetpoint = Rotations.of(10.0);
+
   // Algae scoring
   public static final Angle kProcessorSetpoint =
       Rotations.of(1.49365); // was 4.297 -> 5.964 -> 3.348
   public static final Angle kBargeSetpoint = Rotations.of(44.785); // 41.936
   public static final Angle kBargeHigherThan = Rotations.of(31.0901);
 
+  // Min elevator height before biscuit movement
+  public static final Angle kBiscuitSafeThreshold = Rotations.of(5);
+
   public static TalonFXConfiguration getBothFXConfig() {
     TalonFXConfiguration fxConfig = new TalonFXConfiguration();
 
@@ -114,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/constants/RobotStateConstants.java

@@ -21,6 +21,6 @@ public class RobotStateConstants {
   public static final double kElevatorClimbMax = 11.4;
 
   // Climb LED Thresholds
-  public static final double kClimbAngleSmall = -0.245;
-  public static final double kClimbAngleBig = -0.215;
+  public static final double kClimbAngleSmall = -0.235; // -0.245
+  public static final double kClimbAngleBig = -0.213; // -0.215
 }

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

@@ -49,6 +49,9 @@ public void setIsRemovingAlgae(boolean isRemoving) {
   }
 
   public void setPosition(Angle position, boolean hasAlgae) {
+    if (position.equals(setPoint)) {
+      return;
+    }
     io.setPosition(position, hasAlgae);
     setPoint = position;
   }

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

@@ -4,6 +4,7 @@
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.constants.DriveConstants;
@@ -78,6 +79,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem {
   private boolean hasElevatorAutoCoralPrestaged = false;
 
   private Timer scoringTimer = new Timer();
+  private Angle nextBiscuitSetpoint;
 
   public RobotStateSubsystem(
       AlgaeSubsystem algaeSubsystem,
@@ -240,6 +242,14 @@ public void toggleGetAlgaeOnCycle() {
     ledSubsystem.setGetAlgeaLights(getAlgaeOnCycle);
   }
 
+  public void setBiscuitTransfer(Angle setpoint, boolean overrideThreshold) {
+    if (overrideThreshold
+        || elevatorSubsystem.getPosition().gt(ElevatorConstants.kBiscuitSafeThreshold)) {
+      biscuitSubsystem.setPosition(setpoint, hasAlgae());
+    }
+    nextBiscuitSetpoint = setpoint;
+  }
+
   public void setCurrentLimiting(boolean isCurrentLimiting) {
     this.isCurrentLimiting = isCurrentLimiting;
     ledSubsystem.setCurrentLimiting(isCurrentLimiting);
@@ -301,7 +311,7 @@ public void toStow() {
     driveSubsystem.setIgnoreSticks(false);
 
     if (biscuitSubsystem.isSafeToStow()) {
-      biscuitSubsystem.setPosition(RobotConstants.kStowSetpoint, hasAlgae());
+      setBiscuitTransfer(RobotConstants.kStowSetpoint, true);
       elevatorSubsystem.setPosition(RobotConstants.kElevatorStowSetpoint);
       algaeSubsystem.hold();
 
@@ -312,7 +322,7 @@ public void toStow() {
   }
 
   public void toStowSafe() {
-    biscuitSubsystem.setPosition(RobotConstants.kStowSetpoint, hasAlgae());
+    setBiscuitTransfer(RobotConstants.kStowSetpoint, true);
     driveSubsystem.removeDriveMultiplier();
     driveSubsystem.setIgnoreSticks(false);
     algaeSubsystem.hold();
@@ -321,7 +331,7 @@ public void toStowSafe() {
   }
 
   public void toStowSequential() {
-    biscuitSubsystem.setPosition(RobotConstants.kStowSetpoint, hasAlgae());
+    setBiscuitTransfer(RobotConstants.kStowSetpoint, true);
     driveSubsystem.removeDriveMultiplier();
     driveSubsystem.setIgnoreSticks(false);
     algaeSubsystem.hold();
@@ -331,23 +341,23 @@ public void toStowSequential() {
 
   public void toAutonPrestage() {
     coralLoc = CoralLoc.CORAL;
-    biscuitSubsystem.setPosition(RobotConstants.kPrestageSetpoint, hasAlgae());
+    setBiscuitTransfer(RobotConstants.kPrestageSetpoint, false);
     elevatorSubsystem.setPosition(ElevatorConstants.kAutoPrestageSetpoint);
     funnelSubsystem.stopMotor();
 
     setState(RobotStates.PRESTAGE, true);
   }
 
   public void toFunnelLoad() {
-    biscuitSubsystem.setPosition(RobotConstants.kFunnelSetpoint, hasAlgae());
+    setBiscuitTransfer(RobotConstants.kFunnelSetpoint, true);
     coralSubsystem.intake();
     elevatorSubsystem.setPosition(RobotConstants.kElevatorFunnelSetpoint);
 
     setState(RobotStates.FUNNEL_LOAD, true);
   }
 
   private void toPrestage() {
-    biscuitSubsystem.setPosition(RobotConstants.kPrestageSetpoint, hasAlgae());
+    setBiscuitTransfer(RobotConstants.kPrestageSetpoint, false);
 
     prestagingForAlgae = getAlgaeOnCycle;
 
@@ -415,7 +425,7 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
 
     if (wantAlgae && !algaeSafe) {
       algaeSubsystem.intake();
-      biscuitSubsystem.setPosition(RobotConstants.kPrestageAlgaeSetpoint, hasAlgae());
+      setBiscuitTransfer(RobotConstants.kPrestageAlgaeSetpoint, true);
 
       switch (getAlgaeLevel()) {
         case L2 -> {
@@ -442,11 +452,11 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
 
       switch (getAlgaeLevel()) {
         case L2 -> {
-          biscuitSubsystem.setPosition(RobotConstants.kL2AlgaeSetpoint, hasAlgae());
+          setBiscuitTransfer(RobotConstants.kL2AlgaeSetpoint, true);
           elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeSetpoint);
         }
         case L3 -> {
-          biscuitSubsystem.setPosition(RobotConstants.kL3AlgaeSetpoint, hasAlgae());
+          setBiscuitTransfer(RobotConstants.kL3AlgaeSetpoint, true);
           elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeSetpoint);
         }
         default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
@@ -467,19 +477,19 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
           currentLevel = scoringLevel;
           switch (scoringLevel) {
             case L1 -> {
-              biscuitSubsystem.setPosition(RobotConstants.kL1CoralSetpoint, hasAlgae());
+              setBiscuitTransfer(RobotConstants.kL1CoralSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint);
             }
             case L2 -> {
-              biscuitSubsystem.setPosition(RobotConstants.kL2CoralSetpoint, hasAlgae());
+              setBiscuitTransfer(RobotConstants.kL2CoralSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint);
             }
             case L3 -> {
-              biscuitSubsystem.setPosition(RobotConstants.kL3CoralSetpoint, hasAlgae());
+              setBiscuitTransfer(RobotConstants.kL3CoralSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint);
             }
             case L4 -> {
-              biscuitSubsystem.setPosition(RobotConstants.kL4CoralSetpoint, hasAlgae());
+              setBiscuitTransfer(RobotConstants.kL4CoralSetpoint, true);
               elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint);
             }
           }
@@ -526,7 +536,7 @@ public void toAlgaeFloorPickup() {
           return;
         }
 
-        biscuitSubsystem.setPosition(RobotConstants.kFloorAlgaeSetpoint, hasAlgae());
+        setBiscuitTransfer(RobotConstants.kFloorAlgaeSetpoint, true);
         elevatorSubsystem.setPosition(ElevatorConstants.kFloorAlgaeSetpoint);
 
         setState(RobotStates.FLOOR_ALGAE, true);
@@ -536,7 +546,7 @@ public void toAlgaeFloorPickup() {
           return;
         }
 
-        biscuitSubsystem.setPosition(RobotConstants.kMicAlgaeSetpoint, hasAlgae());
+        setBiscuitTransfer(RobotConstants.kMicAlgaeSetpoint, true);
         elevatorSubsystem.setPosition(ElevatorConstants.kMicAlgaeSetpoint);
 
         setState(RobotStates.MIC_ALGAE, true);
@@ -598,6 +608,7 @@ public void releaseAlgae() {
 
     processorReleasePose = driveSubsystem.getPoseMeters();
     Logger.recordOutput("RobotState/Processor Release Pose", processorReleasePose);
+    driveSubsystem.removeDriveMultiplier();
 
     switch (algaeHeight) {
       case LOW -> {
@@ -623,7 +634,7 @@ public void toHpAlgae() {
 
     algaeSubsystem.intake();
 
-    biscuitSubsystem.setPosition(RobotConstants.kHpAlgaeSetpoint, hasAlgae());
+    setBiscuitTransfer(RobotConstants.kHpAlgaeSetpoint, false);
     elevatorSubsystem.setPosition(ElevatorConstants.kHpAlgaeSetpoint);
 
     setState(RobotStates.HP_ALGAE, true);
@@ -640,7 +651,7 @@ private void toProcessor() {
       return;
     }
 
-    biscuitSubsystem.setPosition(RobotConstants.kProcessorSetpoint, hasAlgae());
+    setBiscuitTransfer(RobotConstants.kProcessorSetpoint, true);
     elevatorSubsystem.setPosition(ElevatorConstants.kProcessorSetpoint);
 
     setState(RobotStates.PROCESSOR_ALGAE, true);
@@ -672,7 +683,7 @@ public void toPrepClimb() {
       return;
     }
 
-    biscuitSubsystem.setPosition(RobotConstants.kStowSetpoint, hasAlgae());
+    setBiscuitTransfer(RobotConstants.kStowSetpoint, true);
     elevatorSubsystem.setPosition(RobotConstants.kElevatorStowSetpoint);
 
     climbSubsystem.prepClimb();
@@ -714,6 +725,9 @@ public void periodic() {
 
     switch (curState) {
       case TRANSFER -> {
+        if (elevatorSubsystem.getPosition().gt(ElevatorConstants.kBiscuitSafeThreshold)) {
+          biscuitSubsystem.setPosition(nextBiscuitSetpoint, hasAlgae());
+        }
         if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()
             || ((nextState == RobotStates.PREP_CLIMB || nextState == RobotStates.CLIMB)
                 && elevatorSubsystem.getPosition().in(Rotations)
@@ -774,11 +788,15 @@ public void periodic() {
           biscuitSubsystem.setIsRemovingAlgae(true);
           switch (getAlgaeLevel()) {
             case L2 -> {
-              biscuitSubsystem.setPosition(RobotConstants.kL2AlgaeRemovalSetpoint, true);
+              biscuitSubsystem.setPosition(
+                  RobotConstants.kL2AlgaeRemovalSetpoint,
+                  true); // not using setBiscuitTransfer() to ensure hasAlgae is true
               elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint);
             }
             case L3 -> {
-              biscuitSubsystem.setPosition(RobotConstants.kL3AlgaeRemovalSetpoint, true);
+              biscuitSubsystem.setPosition(
+                  RobotConstants.kL3AlgaeRemovalSetpoint,
+                  true); // not using setBiscuitTransfer() to ensure hasAlgae is true
               elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeRemovalSetpoint);
             }
             default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
@@ -892,7 +910,9 @@ public void periodic() {
       }
       case TO_BARGE_ALGAE -> {
         if (elevatorSubsystem.isHigherThan(ElevatorConstants.kBargeHigherThan)) {
-          biscuitSubsystem.setPosition(RobotConstants.kBargeSetpoint, hasAlgae());
+          biscuitSubsystem.setPosition(
+              RobotConstants.kBargeSetpoint,
+              hasAlgae()); // not using setBiscuitTransfer(), moves biscuit at higher threshold
           curState = RobotStates.BARGE_ALGAE;
         }
       }

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;