Merge branch 'barge-speedup' into IRI-auton

mwitcpalek · mwitcpalek · commit 13eb816d9b60 · 2025-07-08T10:52:50.000-04:00
diff --git a/src/main/deploy/choreo/bargeToI.traj b/src/main/deploy/choreo/bargeToI.traj
diff --git a/src/main/java/frc/robot/commands/auton/DriveAlgaeAutonServoCommand.java b/src/main/java/frc/robot/commands/auton/DriveAlgaeAutonServoCommand.java
@@ -117,7 +117,7 @@ public void initialize() {
       robotStateSubsystem.clearCoral();
     }
 
-    visionSubsystem.setIsAuto(false);
+    // visionSubsystem.setIsAuto(false);
     robotStateSubsystem.setAutoAlgaeLevel(algaeLevel);
 
     isServoing = false;
diff --git a/src/main/java/frc/robot/commands/auton/MiddleBargeAutonCommand.java b/src/main/java/frc/robot/commands/auton/MiddleBargeAutonCommand.java
@@ -44,7 +44,8 @@ public MiddleBargeAutonCommand(
       List<Double> delays,
       List<String> bargePaths,
       List<RobotStateSubsystem.ScoringLevel> algaeLevels,
-      Pose2d startPose) {
+      Pose2d startPose,
+      double startDelay) {
     addRequirements(
         driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
     this.driveSubsystem = driveSubsystem;
@@ -57,7 +58,8 @@ public MiddleBargeAutonCommand(
 
     addCommands(
         new PrepOdomForAutoCommand(
-            robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose));
+            robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(180.0), startPose),
+        new WaitCommand(startDelay));
 
     for (int i = 0; i < grabPaths.size(); i++) {
       boolean last = i == grabPaths.size() - 1;
diff --git a/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java b/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java
@@ -58,7 +58,7 @@ public boolean isFinished() {
       return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
           || (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
               && !robotStateSubsystem.getIsAutoPlacing())
-          || !robotStateSubsystem.getIsBargeSafe();
+      /*|| !robotStateSubsystem.getIsBargeSafe() */ ;
     }
   }
 }
diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java
@@ -30,7 +30,7 @@ public class AlgaeConstants {
   public static final double kCoralHoldSpeed = -0.05;
   public static final double kBargeScoreSpeed = -1;
   public static final double kProcessorScoreSpeed = -1;
-  public static final double kCoralScoreSpeed = 0.4; // 0.5;
+  public static final double kCoralScoreSpeed = 0.3; // 0.5; was 0.4
   public static final double kIntakingSpeed = 1; // 0.75
   public static final double kCoralIntakingSpeed = -0.3; // -0.75;
 
diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java
@@ -29,7 +29,7 @@ public class AutonConstants {
       new Pose2d(7.1, 7.257, Rotation2d.fromDegrees(180));
 
   public static final Pose2d kMiddleBargeStart =
-      new Pose2d(7.1, 7.257, Rotation2d.fromDegrees(180));
+      new Pose2d(7.1, 3.7209, Rotation2d.fromDegrees(180));
 
   public static final Pose2d kDeepBarge = new Pose2d(7.7, 7.092, Rotation2d.fromDegrees(90));
 }
diff --git a/src/main/java/frc/robot/constants/BargeAlignConstants.java b/src/main/java/frc/robot/constants/BargeAlignConstants.java
@@ -4,11 +4,18 @@
 
 public class BargeAlignConstants {
   public static final double kXSpeed = 0.45;
+  public static final double kXRevSpeed = 2;
 
   public static final double kBlueEjectAlgaeX = 7.67; // 7.72
   public static final double kRedEjectAlgaeX = 9.86; // 9.81
-  public static final double kBlueRaiseElevatorX = 7.02; // 7.22
-  public static final double kRedRaiseElevatorX = 10.51; // 10.31
+  public static final double kBlueRaiseElevatorX = 6.82; // 7.02
+  public static final double kRedRaiseElevatorX = 10.71; // 10.51
+
+  public static final double kBlueRevDoneX = kBlueRaiseElevatorX + 0.25;
+  public static final double kRedRevDoneX = kRedRaiseElevatorX - 0.25;
+
+  public static final double kBlueUnsafeX = 7.02;
+  public static final double kRedUnsafeX = 10.51;
 
   public static final Rotation2d kBlueDesiredYaw = Rotation2d.fromDegrees(0.0);
   public static final Rotation2d kRedDesiredYaw = Rotation2d.fromDegrees(180.0);
diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java
@@ -349,7 +349,7 @@ public static TalonFXSConfiguration getFXSConfig() {
 
   public static class CompConstants {
     public static Angle kElevatorFunnelSetpoint = Rotations.of(0.3676757);
-    public static Angle kElevatorL1LoadSetpoint = Rotations.of(0.278); // 0.63
+    public static Angle kElevatorL1LoadSetpoint = Rotations.of(0.278 + 0.352); // 0.278
     public static Angle kElevatorStowSetpoint = kElevatorFunnelSetpoint;
 
     // Biscuit
@@ -366,7 +366,7 @@ public static class CompConstants {
     // Idle
     public static Angle kBiscuitStowSetpoint = Rotations.of(1.862 / 2);
     public static Angle kFunnelSetpoint = kBiscuitStowSetpoint;
-    public static Angle kL1CoralLoadSetpoint = Rotations.of(6.29); // 0.05 is about an inch
+    public static Angle kL1CoralLoadSetpoint = Rotations.of(6.29 + 0.25); //  was 6.29
     public static Angle kPrestageSetpoint = Rotations.of(-2.94);
     public static Angle kPrestageAlgaeSetpoint = kBiscuitStowSetpoint;
 
@@ -437,12 +437,12 @@ public static TalonFXSConfiguration getFXSConfig() {
 
       CurrentLimitsConfigs current =
           new CurrentLimitsConfigs()
-              .withStatorCurrentLimit(100)
+              .withStatorCurrentLimit(60)
               .withStatorCurrentLimitEnable(true)
               .withSupplyCurrentLimit(20)
               .withSupplyCurrentLowerLimit(5)
               .withSupplyCurrentLowerTime(2)
-              .withSupplyCurrentLimitEnable(true);
+              .withSupplyCurrentLimitEnable(false);
       fxsConfig.CurrentLimits = current;
 
       HardwareLimitSwitchConfigs hwLimit =
diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java
@@ -8,7 +8,7 @@ public class RobotStateConstants {
   public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
   public static final double kAlgaeRetreatDistance = 0;
 
-  public static final double kBlueBargeSafeX = 7; // 7.6
+  public static final double kBlueBargeSafeX = 7.02; // 7.6
   public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
 
   public static final double kCoralEjectTimer = 0.25;
diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java
@@ -283,7 +283,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
                     PathHandlerConstants.kLoadLightDistance,
                     PathHandlerConstants.kLoadLightDistance,
                     PathHandlerConstants.kLoadLightDistance)),
-            new ArrayList<>(Arrays.asList(-0.0175, 0.0, 0.0)),
+            new ArrayList<>(Arrays.asList(-0.0075, -0.0075, -0.0075)),
             'j',
             false,
             AutonConstants.kNonProcessorMid);
@@ -367,10 +367,11 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
             visionSubsystem,
             new ArrayList<String>(Arrays.asList("startBargeToG", "bargeToI", "bargeToE")),
             new ArrayList<Double>(Arrays.asList(0.0, 0.0, 0.0)),
-            new ArrayList<Double>(Arrays.asList(0.5, 0.0, 0.0)),
+            new ArrayList<Double>(Arrays.asList(0.0, 0.0, 0.0)),
             new ArrayList<String>(Arrays.asList("GToBarge", "IToBarge", "EToNearBarge")),
             new ArrayList<>(Arrays.asList(ScoringLevel.L2, ScoringLevel.L3, ScoringLevel.L3)),
-            AutonConstants.kMiddleBargeStart);
+            AutonConstants.kMiddleBargeStart,
+            0.5);
       }
 
       case 0x11 -> {
@@ -384,11 +385,12 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
             tagAlignSubsystem,
             visionSubsystem,
             new ArrayList<String>(Arrays.asList("startBargeToG", "bargeToE", "bargeToI")),
-            new ArrayList<Double>(Arrays.asList(0.0, 0.0, 0.0)),
+            new ArrayList<Double>(Arrays.asList(-0.00475, 0.0, 0.0)),
             new ArrayList<Double>(Arrays.asList(0.0, 0.0, 0.0)),
             new ArrayList<String>(Arrays.asList("GToBarge", "EToBarge", "IToNearBarge")),
             new ArrayList<>(Arrays.asList(ScoringLevel.L2, ScoringLevel.L3, ScoringLevel.L3)),
-            AutonConstants.kMiddleBargeStart);
+            AutonConstants.kMiddleBargeStart,
+            0.0);
       }
 
       case 0x12 -> {
@@ -406,7 +408,8 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
             new ArrayList<Double>(Arrays.asList(0.0, 0.0, 0.0)),
             new ArrayList<String>(Arrays.asList("GToBarge", "IToBarge", "bargeAway")),
             new ArrayList<>(Arrays.asList(ScoringLevel.L2, ScoringLevel.L3, ScoringLevel.L2)),
-            AutonConstants.kMiddleBargeStart);
+            AutonConstants.kMiddleBargeStart,
+            2.0);
       }
 
       case 0x13 -> {
@@ -424,7 +427,8 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
             new ArrayList<Double>(Arrays.asList(0.0, 0.0, 0.0)),
             new ArrayList<String>(Arrays.asList("GToBarge", "EToBarge", "bargeAway")),
             new ArrayList<>(Arrays.asList(ScoringLevel.L2, ScoringLevel.L3, ScoringLevel.L2)),
-            AutonConstants.kMiddleBargeStart);
+            AutonConstants.kMiddleBargeStart,
+            2.0);
       }
 
       case 0x14 -> {
@@ -565,7 +569,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
                     PathHandlerConstants.kLoadLightDistance,
                     PathHandlerConstants.kLoadLightDistance,
                     PathHandlerConstants.kLoadLightDistance)),
-            new ArrayList<>(Arrays.asList(0.0, 0.0, 0.0)),
+            new ArrayList<>(Arrays.asList(-0.0075, -0.0075, -0.0075)),
             'e',
             true,
             AutonConstants.kProcessorMid);
diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFXS.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFXS.java
@@ -6,7 +6,7 @@
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXSConfiguration;
 import com.ctre.phoenix6.configs.TalonFXSConfigurator;
-import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.hardware.TalonFXS;
 import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
 import edu.wpi.first.math.MathUtil;
@@ -42,6 +42,7 @@ public class BiscuitIOFXS implements BiscuitIO, Checkable {
   private StatusSignal<ForwardLimitTypeValue> fwdLimitSwitch;
   private StatusSignal<Angle> rawQuadrature;
   private StatusSignal<Angle> rawPulseWidth;
+  // private StatusSignal<Current> statorCurrent;
   private boolean didZero;
   private boolean fwdLimitSwitchOpen;
   private Angle offset;
@@ -50,8 +51,8 @@ public class BiscuitIOFXS implements BiscuitIO, Checkable {
   private boolean isRemovingAlgae = false;
 
   private TalonFXSConfigurator configurator;
-  private MotionMagicDutyCycle positionRequest =
-      new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0);
+  private MotionMagicVoltage positionRequest =
+      new MotionMagicVoltage(0).withEnableFOC(false).withFeedForward(0);
 
   public BiscuitIOFXS() {
     // Logger initialization with class name
@@ -73,6 +74,8 @@ public BiscuitIOFXS() {
     rawQuadrature.setUpdateFrequency(20);
     rawPulseWidth = talon.getRawPulseWidthPosition();
     rawPulseWidth.setUpdateFrequency(200);
+    // statorCurrent = talon.getStatorCurrent();
+    // statorCurrent.setUpdateFrequency(4);
     zero();
   }
 
diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java
@@ -100,7 +100,16 @@ public void periodic() {
       case NORMAL:
         if (isFinished()
             && FastMath.abs(inputs.velocity) <= BiscuitConstants.kZeroVelThresh
-            && setPoint != prevSetPoint) curState = BiscuitState.CHECK_ZERO;
+            && prevSetPoint != setPoint) {
+          if (FastMath.abs(RobotConstants.kMicAlgaeSetpoint.minus(setPoint).in(Rotations))
+                  < BiscuitConstants.kCloseEnough
+              || FastMath.abs(RobotConstants.kProcessorSetpoint.minus(setPoint).in(Rotations))
+                  < BiscuitConstants.kCloseEnough) {
+            curState = BiscuitState.CHECK_ZERO;
+          } else {
+            prevSetPoint = setPoint;
+          }
+        }
         break;
       case CHECK_ZERO:
         prevSetPoint = setPoint;
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
@@ -105,7 +105,8 @@ public void eject(ScoringLevel level) {
     // setSpeed(CoralConstants.kEjectingSpeed);
 
     switch (level) {
-      case L1, L2, L3 -> setPct(0.8);
+      case L1, L2 -> setPct(0.8);
+      case L3 -> setPct(0.75);
       case L4 -> setPct(1);
     }
     setState(CoralState.EJECTING);
diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java
@@ -741,18 +741,18 @@ public void toScoreAlgae() {
         isBargeSafe =
             poseX > RobotStateConstants.kRedBargeSafeX
                 || poseX < RobotStateConstants.kBlueBargeSafeX;
-        if (isBargeSafe) {
-          driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier);
-          if (isAutoPlacing) {
-            setAutoPlacingLed(true);
-            driveSubsystem.setIgnoreSticks(true);
-            bargeAlignSubsystem.startBargeAlign(allianceColor);
-            setState(RobotStates.BARGE_ALIGN);
-          } else {
-            elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint);
-            setState(RobotStates.TO_BARGE_ALGAE);
-          }
+        // if (isBargeSafe) {
+        driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier);
+        if (isAutoPlacing) {
+          setAutoPlacingLed(true);
+          driveSubsystem.setIgnoreSticks(true);
+          bargeAlignSubsystem.startBargeAlign(allianceColor);
+          setState(RobotStates.BARGE_ALIGN);
+        } else {
+          elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint);
+          setState(RobotStates.TO_BARGE_ALGAE);
         }
+        // }
       }
     }
   }
diff --git a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java
@@ -39,12 +39,12 @@ public BargeAlignSubsystem(FlyskyJoystick flysky, DriveSubsystem driveSubsystem)
 
   public void startBargeAlign(Alliance alliance) {
     this.alliance = alliance;
-    if (isSafe()) {
-      this.isOnBlueSide = isOnBlueSide();
-      setState(BargeAlignStates.DRIVE);
-      setupBargeAlign();
-      driveOmega.reset();
-    }
+    // if (isSafe()) {
+    this.isOnBlueSide = isOnBlueSide();
+    setState(BargeAlignStates.DRIVE);
+    setupBargeAlign();
+    driveOmega.reset();
+    // }
   }
 
   public void killBargeAlign() {
@@ -83,8 +83,24 @@ private boolean isSafe() {
   private boolean shouldRaiseElevator() {
     double poseX = driveSubsystem.getPoseMeters().getX();
     return isOnBlueSide
-        ? poseX > BargeAlignConstants.kBlueRaiseElevatorX
-        : poseX < BargeAlignConstants.kRedRaiseElevatorX;
+        ? (poseX > BargeAlignConstants.kBlueRaiseElevatorX
+            && poseX <= BargeAlignConstants.kBlueUnsafeX)
+        : (poseX < BargeAlignConstants.kRedRaiseElevatorX
+            && poseX >= BargeAlignConstants.kRedUnsafeX);
+  }
+
+  private boolean shouldDriveBackwards() {
+    double poseX = driveSubsystem.getPoseMeters().getX();
+    return isOnBlueSide
+        ? poseX > BargeAlignConstants.kBlueUnsafeX
+        : poseX < BargeAlignConstants.kRedUnsafeX;
+  }
+
+  private boolean shouldStopDrivingBackwards() {
+    double poseX = driveSubsystem.getPoseMeters().getX();
+    return isOnBlueSide
+        ? poseX < BargeAlignConstants.kBlueRevDoneX
+        : poseX > BargeAlignConstants.kRedRevDoneX;
   }
 
   private boolean shouldEjectAlgae() {
@@ -131,6 +147,8 @@ public void periodic() {
         Logger.recordOutput("BargeAlign/Vomega", vOmega);
         if (shouldRaiseElevator()) {
           setState(BargeAlignStates.RAISE_ELEV);
+        } else if (shouldDriveBackwards()) {
+          setState(BargeAlignStates.REVERSE);
         }
       }
       case RAISE_ELEV -> {
@@ -146,14 +164,18 @@ public void periodic() {
         }
       }
       case REVERSE -> {
-        if (isOnBlueSide) {
-          driveSubsystem.move(
-              -vX, getYStickReading(), BargeAlignConstants.kBlueRaiseElevatorX, true);
-          if (shouldRaiseElevator()) {
-            setState(BargeAlignStates.RAISE_ELEV);
-          }
-        } else {
-          driveSubsystem.move(vX, getYStickReading(), BargeAlignConstants.kRedRaiseElevatorX, true);
+        double vOmega =
+            driveOmega.calculate(
+                driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians());
+        double revX = BargeAlignConstants.kXRevSpeed * (isOnBlueSide ? -1 : 1);
+        driveSubsystem.move(revX, getYStickReading(), vOmega, true);
+
+        Logger.recordOutput("BargeAlign/Vx", revX);
+        Logger.recordOutput("BargeAlign/OmegaErr", driveOmega.getError());
+        Logger.recordOutput("BargeAlign/Vomega", vOmega);
+
+        if (shouldStopDrivingBackwards()) {
+          setState(BargeAlignStates.RAISE_ELEV);
         }
       }
       case FINISHED -> {}
diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java

Original file line number	Diff line number	Diff line change
`@@ -117,7 +117,7 @@ public void initialize() {`
`117`	`117`	`robotStateSubsystem.clearCoral();`
`118`	`118`	`}`
`119`	`119`
`120`		`- visionSubsystem.setIsAuto(false);`
	`120`	`+ // visionSubsystem.setIsAuto(false);`
`121`	`121`	`robotStateSubsystem.setAutoAlgaeLevel(algaeLevel);`
`122`	`122`
`123`	`123`	`isServoing = false;`
Original file line number	Diff line number	Diff line change
`@@ -58,7 +58,7 @@ public boolean isFinished() {`
`58`	`58`	`return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE`
`59`	`59`	`\|\| (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE`
`60`	`60`	`&& !robotStateSubsystem.getIsAutoPlacing())`
`61`		`- \|\| !robotStateSubsystem.getIsBargeSafe();`
	`61`	`+ /\|\| !robotStateSubsystem.getIsBargeSafe() / ;`
`62`	`62`	`}`
`63`	`63`	`}`
`64`	`64`	`}`
Original file line number	Diff line number	Diff line change
`@@ -29,7 +29,7 @@ public class AutonConstants {`
`29`	`29`	`new Pose2d(7.1, 7.257, Rotation2d.fromDegrees(180));`
`30`	`30`
`31`	`31`	`public static final Pose2d kMiddleBargeStart =`
`32`		`- new Pose2d(7.1, 7.257, Rotation2d.fromDegrees(180));`
	`32`	`+ new Pose2d(7.1, 3.7209, Rotation2d.fromDegrees(180));`
`33`	`33`
`34`	`34`	`public static final Pose2d kDeepBarge = new Pose2d(7.7, 7.092, Rotation2d.fromDegrees(90));`
`35`	`35`	`}`