refactor(swerve): Recalculate acceleration.

src/main/java/frc/robot/auto/Auto.java

@@ -45,7 +45,7 @@ private Auto() {
         new HolonomicPathFollowerConfig(
             new PIDConstants(1.0),
             new PIDConstants(1.0),
-            SwerveConstants.MAXIMUM_SPEED,
+            SwerveConstants.MAXIMUM_ATTAINABLE_SPEED,
             SwerveConstants.NORTH_EAST_MODULE_CONFIG.position().getNorm(),
             new ReplanningConfig());
 

src/main/java/frc/robot/swerve/Drive.java

@@ -36,7 +36,12 @@ public class Drive extends Command {
   /* Heading feedback controller. */
   private final ProfiledRotationPIDController headingFeedback =
       new ProfiledRotationPIDController(
-          6, 0, 0.1, new Constraints(0.25 * SwerveConstants.MAXIMUM_ROTATION_SPEED, 8));
+          6,
+          0,
+          0.1,
+          new Constraints(
+              SwerveConstants.MAXIMUM_ROTATION_SPEED,
+              SwerveConstants.MAXIMUM_ROTATION_ACCELERATION));
 
   private final DoubleEntry headingEntry,
       headingVelocityEntry,
@@ -123,8 +128,9 @@ public void execute() {
     }
 
     double maxOmegaRadiansPerSecond =
-        Units.rotationsToRadians(SwerveConstants.MAXIMUM_ROTATION_SPEED);
+        Units.rotationsToRadians(SwerveConstants.MAXIMUM_ATTAINABLE_ROTATION_SPEED);
 
+    // Clamp angular velocity
     chassisSpeeds.omegaRadiansPerSecond =
         Math.signum(chassisSpeeds.omegaRadiansPerSecond)
             * Math.min(maxOmegaRadiansPerSecond, Math.abs(chassisSpeeds.omegaRadiansPerSecond));

src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java

@@ -68,7 +68,7 @@ private VoltageOut calculateVelocityVoltage(double velocityMetersPerSecond, bool
    * @return the voltage to apply.
    */
   private VoltageOut calculateOpenLoopVelocityVoltage(double velocityMetersPerSecond) {
-    double velocityPercent = velocityMetersPerSecond / SwerveConstants.MAXIMUM_SPEED;
+    double velocityPercent = velocityMetersPerSecond / SwerveConstants.MAXIMUM_ATTAINABLE_SPEED;
 
     double velocityVolts = velocityPercent * RobotConstants.BATTERY_VOLTAGE;
 

src/main/java/frc/robot/swerve/DriveRequest.java

@@ -88,7 +88,7 @@ public Translation2d getTranslationVelocity() {
   }
 
   public double getRotationVelocity() {
-    return SwerveConstants.MAXIMUM_ROTATION_SPEED * 0.25 * this.rotationVector.getY();
+    return SwerveConstants.MAXIMUM_ROTATION_SPEED * this.rotationVector.getY();
   }
 
   private Rotation2d snapToNearest(Rotation2d angle, Rotation2d multiple) {

src/main/java/frc/robot/swerve/Swerve.java

@@ -155,15 +155,16 @@ public void setChassisSpeeds(ChassisSpeeds speeds) {
    * @param lazy if true, optimize the module setpoint.
    */
   public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(setpoints, SwerveConstants.MAXIMUM_SPEED);
+    SwerveDriveKinematics.desaturateWheelSpeeds(
+        setpoints, SwerveConstants.MAXIMUM_ATTAINABLE_SPEED);
 
     for (int i = 0; i < 4; i++) {
       swerveModules[i].setSetpoint(setpoints[i], lazy);
     }
   }
 
   /**
-   * Set the steer motor setpoints for each of the swerve modules. 
+   * Set the steer motor setpoints for each of the swerve modules.
    *
    * @param steerSetpoints the steer motor setpoints for each swerve module.
    */
@@ -199,7 +200,10 @@ public void pointSideways() {
         Rotation2d.fromDegrees(90.0));
   }
 
-  /** Sets the steer motor setpoints to point the swerve modules inwards, like a cross or "X" pattern. */
+  /**
+   * Sets the steer motor setpoints to point the swerve modules inwards, like a cross or "X"
+   * pattern.
+   */
   public void pointInwards() {
     setSteerSetpoints(
         Rotation2d.fromDegrees(45.0),

src/main/java/frc/robot/swerve/SwerveConstants.java

@@ -75,11 +75,22 @@ public static class MK4iConstants {
           Rotation2d.fromRotations(-0.954346));
 
   /** Maximum attainable speed in meters per second. */
-  public static final double MAXIMUM_SPEED = Units.feetToMeters(18.38719);
+  public static final double MAXIMUM_ATTAINABLE_SPEED = Units.feetToMeters(18.38719);
+
+  /** Maximum speed in meters per second. */
+  public static final double MAXIMUM_SPEED = MAXIMUM_ATTAINABLE_SPEED;
 
   /** Maximum attainable rotational speed in rotations per second. */
-  public static final double MAXIMUM_ROTATION_SPEED =
-      NORTH_WEST_MODULE_CONFIG.position().getNorm() * MAXIMUM_SPEED;
+  public static final double MAXIMUM_ATTAINABLE_ROTATION_SPEED =
+      NORTH_WEST_MODULE_CONFIG.position().getNorm() * MAXIMUM_ATTAINABLE_SPEED;
+
+  /** Maximum rotational speed while snapping to heading in rotations per second. */
+  public static final double MAXIMUM_ROTATION_SPEED = 0.5;
+
+  /**
+   * Maximum rotational acceleration while snapping to heading in rotations per second per second.
+   */
+  public static final double MAXIMUM_ROTATION_ACCELERATION = 8.0;
 
   public static class DriveMotorConstants {
     /** If true, use open-loop control. */