feat(drivetrain): smooth arcade drive inputs with asymmetric acceleration limiter

src/main/java/frc/robot/Constants.java

@@ -4,14 +4,6 @@
 
 package frc.robot;
 
-/**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be declared
- * globally (i.e. public static). Do not put anything functional in this class.
- *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
- * constants are needed, to reduce verbosity.
- */
 public final class Constants {
   public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;
@@ -22,5 +14,23 @@ public static class DrivetrainConstants {
     public static final int BackLeftId = 11;
     public static final int FrontRightId = 14;
     public static final int BackRightId = 13;
+
+    // Max acceleration per second for the drive limiter.
+    public static final double kDriveMaxRatePerSec = 16.0;
+
+    // Power-curve exponent for the drive limiter.
+    public static final double kDriveCurveExponent = 3.5;
+
+    // Maximum power for drive
+    public static final double kDriveMaxPower = 0.8;
+
+    // Max acceleration per second for the turn limiter.
+    public static final double kTurnMaxRatePerSec = 16.0;
+
+    // Power-curve exponent for the turn limiter.
+    public static final double kTurnCurveExponent = 4.5;
+
+    // Maximum power for turn
+    public static final double kTurnMaxPower = 0.8;
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -42,7 +42,9 @@ public RobotContainer() {
    */
   private void configureBindings() {
     drivetrain.setDefaultCommand(
-        drivetrain.arcadeDrive(m_driverController::getLeftX, m_driverController::getLeftY));
+        drivetrain
+            .arcadeDrive(m_driverController::getRightX, m_driverController::getLeftY)
+            .beforeStarting(drivetrain::resetAccelerationLimiters));
   }
 
   /**

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.util.AccelerationLimiter;
 import java.util.function.DoubleSupplier;
 
 public class Drivetrain extends SubsystemBase {
@@ -24,6 +25,15 @@ public class Drivetrain extends SubsystemBase {
 
   private DifferentialDrive differentialDrive;
 
+  private AccelerationLimiter driveLimiter =
+      new AccelerationLimiter(
+          Constants.DrivetrainConstants.kDriveMaxRatePerSec,
+          Constants.DrivetrainConstants.kDriveCurveExponent);
+  private AccelerationLimiter turnLimiter =
+      new AccelerationLimiter(
+          Constants.DrivetrainConstants.kTurnMaxRatePerSec,
+          Constants.DrivetrainConstants.kTurnCurveExponent);
+
   /** Creates a new Drivetrain. */
   public Drivetrain() {
     frontLeft = new SparkMax(Constants.DrivetrainConstants.FrontLeftId, MotorType.kBrushless);
@@ -60,10 +70,26 @@ public Drivetrain() {
   public Command arcadeDrive(DoubleSupplier x, DoubleSupplier y) {
     return run(
         () -> {
-          differentialDrive.arcadeDrive(x.getAsDouble(), y.getAsDouble());
+          double limitedDrive = driveLimiter.calculate(y.getAsDouble());
+          double limitedTurn = turnLimiter.calculate(x.getAsDouble());
+
+          double forward =
+              Math.max(
+                  -Constants.DrivetrainConstants.kDriveMaxPower,
+                  Math.min(limitedDrive, Constants.DrivetrainConstants.kDriveMaxPower));
+          double turn =
+              Math.max(
+                  -Constants.DrivetrainConstants.kTurnMaxPower,
+                  Math.min(limitedTurn, Constants.DrivetrainConstants.kTurnMaxPower));
+          differentialDrive.arcadeDrive(forward, turn);
         });
   }
 
+  public void resetAccelerationLimiters() {
+    driveLimiter.reset();
+    turnLimiter.reset();
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

src/main/java/frc/robot/util/AccelerationLimiter.java

@@ -0,0 +1,64 @@
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj.Timer;
+
+public class AccelerationLimiter {
+  private static final double MAX_DT_SECONDS = 0.1;
+  private static final double RESET_DT_SECONDS = 0.2;
+
+  private final double maxRate;
+  private final double curveExponent;
+
+  private double prevValue;
+  private double prevTime;
+  private boolean firstCall;
+
+  public AccelerationLimiter(double rate, double exponent) {
+    this.maxRate = rate;
+    this.curveExponent = exponent;
+    prevValue = 0.0;
+    prevTime = 0.0;
+    firstCall = true;
+  }
+
+  public double calculate(double input) {
+    double now = Timer.getFPGATimestamp();
+    double dt = now - prevTime;
+
+    if (firstCall || dt > RESET_DT_SECONDS) {
+      prevValue = input;
+      prevTime = now;
+      firstCall = false;
+      return input;
+    }
+
+    if (dt <= 0.0) {
+      return prevValue;
+    }
+    if (dt > MAX_DT_SECONDS) {
+      dt = MAX_DT_SECONDS;
+    }
+
+    double prevMag = Math.abs(prevValue);
+    double inputMag = Math.abs(input);
+
+    if (inputMag < prevMag) {
+      prevValue = input;
+    } else {
+      double distanceFromMax = 1.0 - prevMag;
+      double rate = maxRate * Math.pow(distanceFromMax, curveExponent);
+      double maxChange = rate * dt;
+      double remaining = inputMag - prevMag;
+      double delta = Math.min(remaining, maxChange);
+      double newMag = prevMag + delta;
+      prevValue = Math.signum(input) * newMag;
+    }
+
+    prevTime = now;
+    return prevValue;
+  }
+
+  public void reset() {
+    firstCall = true;
+  }
+}