Worlds hotfixes

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;

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;

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;
 

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

@@ -29,5 +29,5 @@ 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));
 }

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

@@ -7,8 +7,11 @@ public class BargeAlignConstants {
 
   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 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);

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 =

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;

src/main/java/frc/robot/subsystems/auto/AutoSwitch.java

@@ -282,7 +282,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);
@@ -348,10 +348,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 -> {
@@ -365,11 +366,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 -> {
@@ -387,7 +389,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 -> {
@@ -405,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", "EToBarge", "bargeAway")),
             new ArrayList<>(Arrays.asList(ScoringLevel.L2, ScoringLevel.L3, ScoringLevel.L2)),
-            AutonConstants.kMiddleBargeStart);
+            AutonConstants.kMiddleBargeStart,
+            2.0);
       }
 
       case 0x14 -> {
@@ -546,7 +550,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);

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();
   }
 

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;

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);

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

@@ -83,8 +83,17 @@ 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 shouldEjectAlgae() {
@@ -131,6 +140,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 +157,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());
+
+        driveSubsystem.move(-vX, getYStickReading(), vOmega, true);
+
+        Logger.recordOutput("BargeAlign/Vx", vX);
+        Logger.recordOutput("BargeAlign/OmegaErr", driveOmega.getError());
+        Logger.recordOutput("BargeAlign/Vomega", vOmega);
+
+        if (shouldRaiseElevator()) {
+          setState(BargeAlignStates.RAISE_ELEV);
         }
       }
       case FINISHED -> {}

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

@@ -295,11 +295,25 @@ public void startAuto(
   }
 
   private void tagAlign() {
+    boolean blueSide = driveSubsystem.getPoseMeters().getX() < (DriveConstants.kFieldMaxX / 2);
+
     alignX.reset();
     alignY.reset();
 
     curState = TagAlignStates.TAG_ALIGN;
 
+    if (scoreLeft) {
+      visionSubsystem.setUsingLeftCam(false);
+      visionSubsystem.setUsingRightCam(true);
+    } else {
+      visionSubsystem.setUsingLeftCam(true);
+      visionSubsystem.setUsingRightCam(false);
+    }
+
+    visionSubsystem.setTrustedTag(
+        (blueSide ? TagServoingConstants.kBlueTargetTag : TagServoingConstants.kRedTargetTag)
+            [(fieldRelHexant + (blueSide ? 0 : 3)) % 6]);
+
     this.driveXCloseEnough =
         algae
             ? TagServoingConstants.kAlgaeDriveXCloseEnough
@@ -319,6 +333,7 @@ private void tagAlign() {
 
   public void terminate() {
     driveSubsystem.stopDriving();
+    visionSubsystem.setTrustedTag(-1);
     visionSubsystem.setUsingLeftCam(true);
     visionSubsystem.setUsingRightCam(true);
     curState = TagAlignStates.DONE;
@@ -417,25 +432,11 @@ public void periodic() {
             case TAG_ALIGN -> {
               if (justAlgae && (FastMath.abs(alignY.getError()) < driveYCloseEnough)) {
                 finalDrive = true;
-                if (scoreLeft) {
-                  visionSubsystem.setUsingLeftCam(false);
-                  visionSubsystem.setUsingRightCam(true);
-                } else {
-                  visionSubsystem.setUsingLeftCam(true);
-                  visionSubsystem.setUsingRightCam(false);
-                }
               } else {
                 if ((
                     /*isAuto ? true :*/ FastMath.abs(alignX.getError()) < driveXCloseEnough)
                     && FastMath.abs(alignY.getError()) < driveYCloseEnough) {
                   finalDrive = true;
-                  if (scoreLeft) {
-                    visionSubsystem.setUsingLeftCam(false);
-                    visionSubsystem.setUsingRightCam(true);
-                  } else {
-                    visionSubsystem.setUsingLeftCam(true);
-                    visionSubsystem.setUsingRightCam(false);
-                  }
                 }
               }
               if (finalDrive