[drive] Speedup auto align

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

Author: spacey-sooty

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

@@ -77,9 +77,23 @@ 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 ProfiledPIDController xController =
+      new ProfiledPIDController(
+          10,
+          0,
+          0,
+          new TrapezoidProfile.Constraints(
+              getMaxLinearSpeedMetersPerSec(), MAX_LINEAR_ACCELERATION));
+  private final ProfiledPIDController yController =
+      new ProfiledPIDController(
+          10,
+          0,
+          0,
+          new TrapezoidProfile.Constraints(
+              getMaxLinearSpeedMetersPerSec(), MAX_LINEAR_ACCELERATION));
+  private final ProfiledPIDController headingController =
+      new ProfiledPIDController(
+          10, 0, 0, new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION));
 
   private final PIDController xFollower = new PIDController(1, 0, 0);
   private final PIDController yFollower = new PIDController(1, 0, 0);
@@ -145,9 +159,9 @@ public Drive(
             new SysIdRoutine.Mechanism(
                 (voltage) -> runSteerCharacterization(voltage.in(Volts)), null, this));
 
-    xController.setTolerance(0.02);
-    yController.setTolerance(0.02);
-    headingController.setTolerance(0.02);
+    xController.setTolerance(0.01);
+    yController.setTolerance(0.01);
+    headingController.setTolerance(0.01);
     headingController.enableContinuousInput(-Math.PI, Math.PI);
 
     headingFollower.enableContinuousInput(-Math.PI, Math.PI);

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 =