refactor(swerve): Continue moving away from constants.

src/main/java/frc/lib/config/MotionProfileConfig.java

@@ -1,5 +1,8 @@
 package frc.lib.config;
 
+import java.util.function.Function;
+
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
@@ -66,11 +69,20 @@ public double maximumAcceleration() {
   }
 
   /**
-   * Creates a new rate limiter using this motion profile config.
+   * Creates a new velocity clamper using this motion profile config.
+   * 
+   * @return a new velocity clamper using this motion profile config.
+   */
+  public Function<Double, Double> createVelocityClamper() {
+    return velocity -> MathUtil.clamp(velocity, -maximumVelocity, maximumVelocity);
+  }
+
+  /**
+   * Creates a new acceleration limiter using this motion profile config.
    *
-   * @return a new rate limiter using this motion profile config.
+   * @return a new acceleration limiter using this motion profile config.
    */
-  public SlewRateLimiter createRateLimiter() {
+  public SlewRateLimiter createAccelerationLimiter() {
     return new SlewRateLimiter(maximumAcceleration);
   }
 

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

@@ -14,7 +14,6 @@
 import frc.robot.odometry.Odometry;
 import frc.robot.superstructure.Superstructure;
 import frc.robot.swerve.Swerve;
-import frc.robot.swerve.SwerveConstants;
 import frc.robot.swerve.SwerveFactory;
 
 /** Auto subsystem. */
@@ -49,7 +48,7 @@ private Auto() {
         new HolonomicPathFollowerConfig(
             new PIDConstants(1.0),
             new PIDConstants(1.0),
-            SwerveConstants.MAXIMUM_SPEED,
+            swerve.maximumTranslationVelocity(),
             SwerveFactory.createNorthEastModuleTranslation().getNorm(), // TODO
             new ReplanningConfig()),
         AllianceFlipHelper::shouldFlip,

src/main/java/frc/robot/shooter/Shooter.java

@@ -91,14 +91,14 @@ private Shooter() {
 
     flywheelValues = new VelocityControllerIOValues();
 
-    flywheelAccelerationLimiter = flywheelConfig.motionProfileConfig().createRateLimiter();
+    flywheelAccelerationLimiter = flywheelConfig.motionProfileConfig().createAccelerationLimiter();
 
     serializer = ShooterFactory.createSerializer(serializerConfig);
     serializer.configure();
 
     serializerValues = new VelocityControllerIOValues();
 
-    serializerAccelerationLimiter = serializerConfig.motionProfileConfig().createRateLimiter();
+    serializerAccelerationLimiter = serializerConfig.motionProfileConfig().createAccelerationLimiter();
 
     setpoint = ShooterState.IDLING;
     goal = ShooterState.IDLING;

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

@@ -1,10 +1,12 @@
 package frc.robot.swerve;
 
-import edu.wpi.first.math.MathUtil;
+import java.util.function.Function;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.RobotConstants;
 import frc.robot.odometry.Odometry;
 
 /** Drives the swerve using driver input. */
@@ -18,8 +20,14 @@ public class DriveCommand extends Command {
   /** Controller used to get driver input. */
   private final CommandXboxController driverController;
 
-  /** Previous requested chassis speed. */
-  private ChassisSpeeds previousChassisSpeeds;
+  /** Translation acceleration limiter. */
+  private final SlewRateLimiter xAccelerationLimiter;
+
+  /** Translation acceleration limiter. */
+  private final SlewRateLimiter yAccelerationLimiter;
+
+  /** Rotation velocity clamper. */
+  private final Function<Double, Double> rotationVelocityClamper;
 
   /** Request from the driver controller. */
   private DriveRequest request;
@@ -37,7 +45,10 @@ public DriveCommand(CommandXboxController driverController) {
 
     addRequirements(swerve);
 
-    previousChassisSpeeds = new ChassisSpeeds();
+    xAccelerationLimiter = swerve.translationMotionProfileConfig().createAccelerationLimiter();
+    yAccelerationLimiter = swerve.translationMotionProfileConfig().createAccelerationLimiter();
+
+    rotationVelocityClamper = swerve.rotationMotionProfileConfig().createVelocityClamper();
 
     request = DriveRequest.fromController(driverController);
   }
@@ -52,14 +63,12 @@ public void execute() {
     final ChassisSpeeds chassisSpeeds =
         clampChassisSpeeds(
             ChassisSpeeds.fromFieldRelativeSpeeds(
-                request.velocity().getX(),
-                request.velocity().getY(),
-                request.omega().getRadians(),
+                request.translationAxis().getX() * swerve.maximumTranslationVelocity(),
+                request.translationAxis().getY() * swerve.maximumTranslationVelocity(),
+                swerve.maximumRotationVelocity().times(request.rotationVelocityAxis()).getRadians(),
                 odometry.getDriverRelativeHeading()));
 
     swerve.setChassisSpeeds(chassisSpeeds);
-
-    previousChassisSpeeds = chassisSpeeds;
   }
 
   @Override
@@ -77,27 +86,9 @@ public boolean isFinished() {
    * @return the clamped chassis speeds.
    */
   private ChassisSpeeds clampChassisSpeeds(ChassisSpeeds desiredChassisSpeeds) {
-    double vxMetersPerSecond =
-        MathUtil.clamp(
-            desiredChassisSpeeds.vxMetersPerSecond,
-            previousChassisSpeeds.vxMetersPerSecond
-                - SwerveConstants.MAXIMUM_ACCELERATION * RobotConstants.PERIODIC_DURATION,
-            previousChassisSpeeds.vxMetersPerSecond
-                + SwerveConstants.MAXIMUM_ACCELERATION * RobotConstants.PERIODIC_DURATION);
-
-    double vyMetersPerSecond =
-        MathUtil.clamp(
-            desiredChassisSpeeds.vyMetersPerSecond,
-            previousChassisSpeeds.vyMetersPerSecond
-                - SwerveConstants.MAXIMUM_ACCELERATION * RobotConstants.PERIODIC_DURATION,
-            previousChassisSpeeds.vyMetersPerSecond
-                + SwerveConstants.MAXIMUM_ACCELERATION * RobotConstants.PERIODIC_DURATION);
-
-    double omegaRadiansPerSecond =
-        MathUtil.clamp(
-            desiredChassisSpeeds.omegaRadiansPerSecond,
-            -SwerveConstants.MAXIMUM_ROTATION_SPEED.getRadians(),
-            SwerveConstants.MAXIMUM_ROTATION_SPEED.getRadians());
+    double vxMetersPerSecond = xAccelerationLimiter.calculate(desiredChassisSpeeds.vxMetersPerSecond);
+    double vyMetersPerSecond = yAccelerationLimiter.calculate(desiredChassisSpeeds.vyMetersPerSecond);
+    double omegaRadiansPerSecond = rotationVelocityClamper.apply(Units.radiansToRotations(desiredChassisSpeeds.omegaRadiansPerSecond));
 
     return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
   }

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

@@ -10,7 +10,8 @@ public record DriveRequest(
     DriveRequest.TranslationMode translationMode,
     DriveRequest.RotationMode rotationMode,
     Translation2d translationAxis,
-    Translation2d rotationAxis) {
+    Translation2d headingAxis,
+    double rotationVelocityAxis) {
 
   /** Translation mode. */
   private enum TranslationMode {
@@ -33,16 +34,16 @@ private enum RotationMode {
   /**
    * Returns true if no rotation is requested.
    *
-   * @param heading the heading axis.
+   * @param headingAxis the heading axis.
    * @param aligning true if aligning is requested.
-   * @return
+   * @return true if no rotation is requested.
    */
-  private static boolean isDrifting(Translation2d heading, boolean aligning) {
+  private static boolean isDrifting(Translation2d headingAxis, boolean aligning) {
     if (aligning) {
-      return heading.getNorm() < 0.7;
+      return headingAxis.getNorm() < 0.7;
     }
 
-    return Math.abs(heading.getY()) < 0.1;
+    return Math.abs(headingAxis.getY()) < 0.1;
   }
 
   /**
@@ -70,43 +71,29 @@ public static DriveRequest fromController(CommandXboxController controller) {
       translationMagnitude *= 0.25;
     }
 
-    Translation2d translationVector = new Translation2d(translationMagnitude, translationDirection);
+    Translation2d translationAxis = new Translation2d(translationMagnitude, translationDirection);
 
     TranslationMode translationMode = TranslationMode.FIELD_CENTRIC;
 
-    Translation2d rotationVector =
+    Translation2d headingAxis =
         new Translation2d(-controller.getRightY(), -controller.getRightX());
 
     RotationMode rotationMode;
 
-    if (isDrifting(rotationVector, aligningRequested)) {
+    if (isDrifting(headingAxis, aligningRequested)) {
       rotationMode = RotationMode.DRIFTING;
     } else if (aligningRequested) {
       rotationMode = RotationMode.ALIGNING;
     } else {
       rotationMode = RotationMode.SPINNING;
     }
 
-    return new DriveRequest(translationMode, rotationMode, translationVector, rotationVector);
-  }
-
-  /**
-   * Returns the requested translation velocity in meters per second.
-   *
-   * @return the requested translation velocity in meters per second.
-   */
-  public Translation2d velocity() {
-    return translationAxis.times(SwerveConstants.MAXIMUM_SPEED);
-  }
+    double rotationVelocityAxis = 0.0;
 
-  /**
-   * Returns the requested rotation velocity.
-   *
-   * @return the requested rotation velocity.
-   */
-  public Rotation2d omega() {
-    if (Math.abs(rotationAxis.getY()) < 0.1) return new Rotation2d();
+    if (rotationMode == RotationMode.SPINNING) {
+      rotationVelocityAxis = headingAxis.getY();
+    }
 
-    return SwerveConstants.MAXIMUM_ROTATION_SPEED.times(rotationAxis.getY());
+    return new DriveRequest(translationMode, rotationMode, translationAxis, headingAxis, rotationVelocityAxis);
   }
 }

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

@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
+import frc.lib.config.MotionProfileConfig;
 import frc.lib.controller.SwerveModuleIO;
 import frc.robot.RobotConstants;
 
@@ -26,6 +27,12 @@ public class Swerve extends Subsystem {
   /** Swerve kinematics. */
   private final SwerveDriveKinematics swerveKinematics;
 
+  /** Translation motion profile config. */
+  private final MotionProfileConfig translationMotionProfileConfig = new MotionProfileConfig().withMaximumVelocity(4.5).withMaximumAcceleration(18);
+
+  /** Rotation motion profile config. */
+  private final MotionProfileConfig rotationMotionProfileConfig = new MotionProfileConfig().withMaximumVelocity(0.25);
+
   /** Initializes the swerve subsystem and configures swerve hardware. */
   private Swerve() {
     swerveModules[0] = SwerveFactory.createNorthWestModule();
@@ -61,15 +68,15 @@ public void periodic() {}
   public void addToShuffleboard(ShuffleboardTab tab) {
     ShuffleboardLayout translationConstants = Telemetry.addColumn(tab, "Translation Constants");
 
-    translationConstants.addDouble("Maximum Velocity (mps)", () -> SwerveConstants.MAXIMUM_SPEED);
+    translationConstants.addDouble("Maximum Velocity (mps)", this::maximumTranslationVelocity);
     translationConstants.addDouble(
-        "Maximum Accleration (mpsps)", () -> SwerveConstants.MAXIMUM_ACCELERATION);
+        "Maximum Accleration (mpsps)", this::maximumTranslationAcceleration);
     translationConstants.addDouble(
         "Wheel Circumference (m)", () -> SwerveConstants.MK4iConstants.WHEEL_CIRCUMFERENCE);
 
     ShuffleboardLayout rotationConstants = Telemetry.addColumn(tab, "Rotation Constants");
     rotationConstants.addDouble(
-        "Maximum Velocity (rps)", () -> SwerveConstants.MAXIMUM_ROTATION_SPEED.getRotations());
+        "Maximum Velocity (rps)", () -> maximumRotationVelocity().getRotations());
 
     Telemetry.addSwerveModuleStates(tab, "Swerve Module States", this::getModuleStates);
     Telemetry.addSwerveModuleStates(tab, "Swerve Module Setpoints", this::getModuleSetpoints);
@@ -172,13 +179,58 @@ public void setChassisSpeeds(ChassisSpeeds speeds) {
    * @param lazy if true, optimize the module setpoints.
    */
   public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(setpoints, SwerveConstants.MAXIMUM_SPEED);
+    SwerveDriveKinematics.desaturateWheelSpeeds(setpoints, maximumTranslationVelocity());
 
     for (int i = 0; i < 4; i++) {
       swerveModules[i].setSetpoint(setpoints[i], lazy);
     }
   }
 
+  /**
+   * Returns the motion profile config.
+   * 
+   * @return the motion profile config.
+   */
+  public MotionProfileConfig translationMotionProfileConfig() {
+    return translationMotionProfileConfig;
+  }
+
+  /**
+   * Returns the maximum translation velocity.
+   * 
+   * @return the maximum translation velocity.
+   */
+  public double maximumTranslationVelocity() {
+    return translationMotionProfileConfig.maximumVelocity();
+  }
+
+  /**
+   * Returns the maximum translation acceleration.
+   * 
+   * @return the maximum translation acceleration.
+   */
+  public double maximumTranslationAcceleration() {
+    return translationMotionProfileConfig.maximumAcceleration();
+  }
+
+  /**
+   * Returns the rotation motion profile config.
+   * 
+   * @return the rotation motion profile config.
+   */
+  public MotionProfileConfig rotationMotionProfileConfig() {
+    return rotationMotionProfileConfig;
+  }
+
+  /**
+   * Returns the maximum rotation velocity.
+   * 
+   * @return the maximum rotation velocity.
+   */
+  public Rotation2d maximumRotationVelocity() {
+    return Rotation2d.fromRotations(rotationMotionProfileConfig.maximumVelocity());
+  }
+
   /**
    * Drives the swerve using an Xbox controller.
    *

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

@@ -1,6 +1,5 @@
 package frc.robot.swerve;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import frc.lib.config.FeedbackControllerConfig;
 import frc.lib.config.FeedforwardControllerConfig;
@@ -25,21 +24,6 @@ public static class MK4iConstants {
     public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI;
   }
 
-  /** Module X offset in meters. */
-  public static final double X_OFFSET = Units.inchesToMeters(10.375);
-
-  /** Module Y offset in meters. */
-  public static final double Y_OFFSET = Units.inchesToMeters(10.375);
-
-  /** Maximum speed in meters per second. */
-  public static final double MAXIMUM_SPEED = 4.5;
-
-  /** Maximum acceleration in meters per second per second. */
-  public static final double MAXIMUM_ACCELERATION = 18;
-
-  /** Maximum rotational speed. */
-  public static final Rotation2d MAXIMUM_ROTATION_SPEED = Rotation2d.fromRotations(0.25);
-
   /** Drive motor config. */
   public static final MechanismConfig DRIVE_CONFIG =
       new MechanismConfig()