[drive] Speedup auto align

Signed-off-by: Jade Turner <spacey-sooty@proton.me>

Author: spacey-sooty

.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
   "enableCppIntellisense": false,
   "currentLanguage": "java",
-  "projectYear": "2025beta",
+  "projectYear": "2025",
   "teamNumber": 4788
 }

src/main/java/org/curtinfrc/frc2025/Constants.java

@@ -15,7 +15,7 @@
  * (log replay from a file).
  */
 public final class Constants {
-  public static final RobotType robotType = RobotType.COMPBOT;
+  public static final RobotType robotType = RobotType.SIMBOT;
   public static final double ROBOT_X = 0.705;
   public static final double ROBOT_Y = 0.730;
   public static final double FIELD_LENGTH = aprilTagLayout.getFieldLength();

src/main/java/org/curtinfrc/frc2025/Robot.java

@@ -299,6 +299,8 @@ public Robot() {
 
     autoChooser.addCmd("Test Auto", this::testAuto);
 
+    autoChooser.addCmd("Align to A", () -> drive.autoAlign(A::getPose));
+
     // Set up SysId routines
     autoChooser.addCmd(
         "Drive Wheel Radius Characterization", () -> drive.wheelRadiusCharacterization());

src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java

@@ -29,6 +29,7 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
@@ -77,33 +78,34 @@ public class Drive extends SubsystemBase {
   private SwerveDrivePoseEstimator poseEstimator =
       new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero);
 
-  private final PIDController xController = new PIDController(3.5, 0, 0);
-  private final PIDController yController = new PIDController(3.5, 0, 0);
-  private final PIDController headingController = new PIDController(3.5, 0, 0);
+  private final TrapezoidProfile magnitudeProfile =
+      new TrapezoidProfile(
+          new TrapezoidProfile.Constraints(
+              getMaxLinearSpeedMetersPerSec(), MAX_LINEAR_ACCELERATION));
+  private final TrapezoidProfile headingController =
+      new TrapezoidProfile(
+          new TrapezoidProfile.Constraints(getMaxAngularSpeedRadPerSec(), ANGLE_MAX_ACCELERATION));
 
   private final PIDController xFollower = new PIDController(1, 0, 0);
   private final PIDController yFollower = new PIDController(1, 0, 0);
   private final PIDController headingFollower = new PIDController(1.5, 0, 0);
 
   @AutoLogOutput(key = "Drive/Setpoint")
-  public DriveSetpoints setpoint = DriveSetpoints.A;
+  public Pose2d setpoint = Pose2d.kZero;
 
   @AutoLogOutput(key = "Drive/AtSetpoint")
   public Trigger atSetpoint =
       new Trigger(
-          () ->
-              xController.atSetpoint()
-                  && yController.atSetpoint()
-                  && headingController.atSetpoint());
-
-  @AutoLogOutput(key = "Drive/AlmostAtSetpoint")
-  public Trigger almostAtSetpoint =
-      new Trigger(
-          () -> {
-            return getPose().minus(setpoint.getPose()).getTranslation().getNorm() < 1;
-          });
-
-  private final RepulsorFieldPlanner repulsorFieldPlanner = new RepulsorFieldPlanner();
+              () -> {
+                var error = setpoint.minus(getPose());
+                Logger.recordOutput("Drive/error", error);
+                var translationOk = Math.abs(error.getTranslation().getNorm()) < 0.03;
+                var rotationOk = Math.abs(error.getRotation().getRadians()) < 0.05;
+                Logger.recordOutput("Drive/translationOK", translationOk);
+                Logger.recordOutput("Drive/rotationOK", rotationOk);
+                return translationOk && rotationOk;
+              })
+          .debounce(0);
 
   public Drive(
       GyroIO gyroIO,
@@ -145,11 +147,6 @@ public Drive(
             new SysIdRoutine.Mechanism(
                 (voltage) -> runSteerCharacterization(voltage.in(Volts)), null, this));
 
-    xController.setTolerance(0.02);
-    yController.setTolerance(0.02);
-    headingController.setTolerance(0.02);
-    headingController.enableContinuousInput(-Math.PI, Math.PI);
-
     headingFollower.enableContinuousInput(-Math.PI, Math.PI);
   }
 
@@ -242,10 +239,6 @@ public void periodic() {
 
     // Update gyro alert
     gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM);
-
-    Logger.recordOutput("xPidAtSetpoint", xController.atSetpoint());
-    Logger.recordOutput("yPidAtSetpoint", yController.atSetpoint());
-    Logger.recordOutput("omegaPidAtSetpoint", headingController.atSetpoint());
   }
 
   /**
@@ -664,7 +657,6 @@ public Command autoAlignWithOverride(
       DoubleSupplier omegaSupplier) {
     return run(
         () -> {
-          this.setpoint = _setpoint.get();
           if (Math.abs(xSupplier.getAsDouble()) > 0.05
               || Math.abs(ySupplier.getAsDouble()) > 0.05
               || Math.abs(omegaSupplier.getAsDouble()) > 0.05) {
@@ -676,16 +668,29 @@ public Command autoAlignWithOverride(
   }
 
   private void autoAlign(Pose2d _setpoint) {
-    Logger.recordOutput("Drive/AutoAlignSetpoint", _setpoint);
+    setpoint = _setpoint;
     var robotPose = getPose();
+    var robotSpeed = getChassisSpeeds();
+    var robotSpeedV = new Translation2d(robotSpeed.vxMetersPerSecond, robotSpeed.vyMetersPerSecond);
+
+    var mag =
+        magnitudeProfile.calculate(
+            0.02,
+            new State(robotPose.getTranslation().getNorm(), robotSpeedV.getNorm()),
+            new State(setpoint.getTranslation().getNorm(), 0));
+    var dir = setpoint.getTranslation().minus(robotPose.getTranslation()).getAngle();
+    var targetSpeed = new Translation2d(Math.abs(mag.velocity), dir);
 
     var omega =
         headingController.calculate(
-            getRotation().getRadians(), _setpoint.getRotation().getRadians());
-    var x = xController.calculate(robotPose.getX(), _setpoint.getX());
-    var y = yController.calculate(robotPose.getY(), _setpoint.getY());
+            0.02,
+            new State(getRotation().getRadians(), robotSpeed.omegaRadiansPerSecond),
+            new State(setpoint.getRotation().getRadians(), 0));
+
     runVelocity(
-        ChassisSpeeds.fromFieldRelativeSpeeds(-x, -y, -omega, getRotation()), new double[4]);
+        ChassisSpeeds.fromFieldRelativeSpeeds(
+            targetSpeed.getX(), targetSpeed.getY(), omega.velocity, getRotation()),
+        new double[4]);
   }
 
   public Command autoAlign(Supplier<Pose2d> _setpoint) {

src/main/java/org/curtinfrc/frc2025/subsystems/drive/DriveConstants.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.util.struct.Struct;
 import edu.wpi.first.util.struct.StructGenerator;
 import edu.wpi.first.util.struct.StructSerializable;
@@ -30,6 +31,10 @@ public final class DriveConstants {
   public static final double FF_RAMP_RATE = 0.1; // Volts/Sec
   public static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec
   public static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2
+  public static final double KT = DCMotor.getKrakenX60Foc(1).KtNMPerAmp * 5.99;
+  // max acceleration = 4 * torque / r / robotWeight
+  // torque = kT * statorCurrentLimit * gearRatio
+  public static final double MAX_LINEAR_ACCELERATION = 4 * KT * 60 / 0.0508 / 38.5554;
   public static final double ODOMETRY_FREQUENCY =
       new CANBus(CompTunerConstants.DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0;
   public static final double DRIVE_BASE_RADIUS =