From 63f140f666f19d0015b8425dc7a919867f773645 Mon Sep 17 00:00:00 2001 From: Aarav Sharma Date: Wed, 24 Jun 2026 19:38:40 -0600 Subject: [PATCH 1/8] feat(drivetrain): add slew rate limiters to arcade drive inputs Smooth driver control by limiting the rate of change of forward and turn inputs via SlewRateLimiter filters (0.5 and 0.3 respectively). --- src/main/java/frc/robot/subsystems/Drivetrain.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 0c3634d..5bf4e55 100755 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -10,6 +10,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -24,6 +25,9 @@ public class Drivetrain extends SubsystemBase { private DifferentialDrive differentialDrive; + private SlewRateLimiter driveFilter = new SlewRateLimiter(0.5); + private SlewRateLimiter turnFilter = new SlewRateLimiter(0.3); + /** Creates a new Drivetrain. */ public Drivetrain() { frontLeft = new SparkMax(Constants.DrivetrainConstants.FrontLeftId, MotorType.kBrushless); @@ -60,7 +64,8 @@ public Drivetrain() { public Command arcadeDrive(DoubleSupplier x, DoubleSupplier y) { return run( () -> { - differentialDrive.arcadeDrive(x.getAsDouble(), y.getAsDouble()); + differentialDrive.arcadeDrive( + driveFilter.calculate(x.getAsDouble()), turnFilter.calculate(y.getAsDouble())); }); } From afb158d152549fffcf54d8c745bf61d6c87d66e7 Mon Sep 17 00:00:00 2001 From: Aarav Sharma Date: Wed, 24 Jun 2026 20:14:29 -0600 Subject: [PATCH 2/8] refactor(drivetrain): move slew rate values to Constants --- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/subsystems/Drivetrain.java | 6 ++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c0510ac..d810a04 100755 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,5 +22,8 @@ public static class DrivetrainConstants { public static final int BackLeftId = 11; public static final int FrontRightId = 14; public static final int BackRightId = 13; + + public static final double kDriveSlewRate = 0.5; + public static final double kTurnSlewRate = 0.3; } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 5bf4e55..4a558a4 100755 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -25,8 +25,10 @@ public class Drivetrain extends SubsystemBase { private DifferentialDrive differentialDrive; - private SlewRateLimiter driveFilter = new SlewRateLimiter(0.5); - private SlewRateLimiter turnFilter = new SlewRateLimiter(0.3); + private SlewRateLimiter driveFilter = + new SlewRateLimiter(Constants.DrivetrainConstants.kDriveSlewRate); + private SlewRateLimiter turnFilter = + new SlewRateLimiter(Constants.DrivetrainConstants.kTurnSlewRate); /** Creates a new Drivetrain. */ public Drivetrain() { From 6457cf255f412a77469c41fa474f0d3abb88671e Mon Sep 17 00:00:00 2001 From: Aarav Sharma Date: Wed, 1 Jul 2026 11:45:59 -0600 Subject: [PATCH 3/8] feat(drivetrain): add asymmetric acceleration limiter to arcade drive Replaces SlewRateLimiter with a custom AccelerationLimiter (frc.robot.util) that operates on magnitude, snaps on deceleration, and applies a power curve on remaining distance for acceleration. Drive: maxRate=44, exponent=3 (~0.6 instant, ~0.85 in 0.5s). Turn: maxRate=40, exponent=4 (~0.5 instant, ~0.75 in 0.5s). Also fixes the arcadeDrive(xSpeed, zRotation) axis binding: previously getLeftX (turn) was passed as xSpeed and getLeftY (forward) as zRotation, so pushing forward made the robot turn. Wires Drivetrain.resetAccelerationLimiters() via .beforeStarting(...) on the default command so enable cycles don't leak limiter state. --- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/subsystems/Drivetrain.java | 24 +++++-- .../frc/robot/util/AccelerationLimiter.java | 64 +++++++++++++++++++ 4 files changed, 88 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/util/AccelerationLimiter.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d810a04..514c476 100755 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,7 +23,9 @@ public static class DrivetrainConstants { public static final int FrontRightId = 14; public static final int BackRightId = 13; - public static final double kDriveSlewRate = 0.5; - public static final double kTurnSlewRate = 0.3; + public static final double kDriveMaxRatePerSec = 44.0; + public static final double kDriveCurveExponent = 3.0; + public static final double kTurnMaxRatePerSec = 40.0; + public static final double kTurnCurveExponent = 4.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8e14214..693612c 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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::getLeftX, m_driverController::getLeftY) + .beforeStarting(drivetrain::resetAccelerationLimiters)); } /** diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 4a558a4..5e26cc0 100755 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -10,11 +10,11 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.drive.DifferentialDrive; 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 { @@ -25,10 +25,14 @@ public class Drivetrain extends SubsystemBase { private DifferentialDrive differentialDrive; - private SlewRateLimiter driveFilter = - new SlewRateLimiter(Constants.DrivetrainConstants.kDriveSlewRate); - private SlewRateLimiter turnFilter = - new SlewRateLimiter(Constants.DrivetrainConstants.kTurnSlewRate); + 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() { @@ -66,11 +70,17 @@ public Drivetrain() { public Command arcadeDrive(DoubleSupplier x, DoubleSupplier y) { return run( () -> { - differentialDrive.arcadeDrive( - driveFilter.calculate(x.getAsDouble()), turnFilter.calculate(y.getAsDouble())); + double forward = driveLimiter.calculate(y.getAsDouble()); + double turn = turnLimiter.calculate(x.getAsDouble()); + differentialDrive.arcadeDrive(forward, turn); }); } + public void resetAccelerationLimiters() { + driveLimiter.reset(); + turnLimiter.reset(); + } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/util/AccelerationLimiter.java b/src/main/java/frc/robot/util/AccelerationLimiter.java new file mode 100644 index 0000000..d4461dc --- /dev/null +++ b/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; + } +} From 3056bd7e98a64ca500025fde4befaeff037fd59c Mon Sep 17 00:00:00 2001 From: Aarav Sharma Date: Wed, 1 Jul 2026 11:52:34 -0600 Subject: [PATCH 4/8] chore(drivetrain): document acceleration limiter constants --- src/main/java/frc/robot/Constants.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 514c476..551ebdc 100755 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,9 +23,18 @@ public static class DrivetrainConstants { public static final int FrontRightId = 14; public static final int BackRightId = 13; + // Peak rate (units/sec) for the drive (throttle) acceleration limiter. public static final double kDriveMaxRatePerSec = 44.0; + + // Power-curve exponent for the drive limiter. Higher = punchier start, softer finish. + // With the values above, 0 -> 0.6 takes ~1 tick and 0.6 -> 0.85 takes ~0.5s. public static final double kDriveCurveExponent = 3.0; + + // Peak rate (units/sec) for the turn (rotation) acceleration limiter. public static final double kTurnMaxRatePerSec = 40.0; + + // Power-curve exponent for the turn limiter. Higher = punchier start, softer finish. + // With the values above, 0 -> 0.5 takes ~1 tick and 0.5 -> 0.75 takes ~0.5s. public static final double kTurnCurveExponent = 4.0; } } From 57ba98db600c3995e8eab13dd7f5bee194c3034b Mon Sep 17 00:00:00 2001 From: Aarav Sharma Date: Wed, 1 Jul 2026 14:12:17 -0600 Subject: [PATCH 5/8] feat(drivetrain): add max power clamps and tune limiter constants - Clamp drive/turn outputs to +/- kDriveMaxPower/kTurnMaxPower - Fix Math.clamp min/max argument order - Tune acceleration limiter rates, curve exponents, and max power - Switch arcade drive turn axis to right stick X --- src/main/java/frc/robot/Constants.java | 32 ++++++++----------- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 7 ++-- 3 files changed, 20 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 551ebdc..9ae336b 100755 --- a/src/main/java/frc/robot/Constants.java +++ b/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. - * - *

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; @@ -23,18 +15,22 @@ public static class DrivetrainConstants { public static final int FrontRightId = 14; public static final int BackRightId = 13; - // Peak rate (units/sec) for the drive (throttle) acceleration limiter. - public static final double kDriveMaxRatePerSec = 44.0; + // Max acceleration per second for the drive limiter. + public static final double kDriveMaxRatePerSec = 8.0; - // Power-curve exponent for the drive limiter. Higher = punchier start, softer finish. - // With the values above, 0 -> 0.6 takes ~1 tick and 0.6 -> 0.85 takes ~0.5s. - public static final double kDriveCurveExponent = 3.0; + // Power-curve exponent for the drive limiter. + public static final double kDriveCurveExponent = 3.5; - // Peak rate (units/sec) for the turn (rotation) acceleration limiter. - public static final double kTurnMaxRatePerSec = 40.0; + // Maximum power for drive + public static final double kDriveMaxPower = 0.5; - // Power-curve exponent for the turn limiter. Higher = punchier start, softer finish. - // With the values above, 0 -> 0.5 takes ~1 tick and 0.5 -> 0.75 takes ~0.5s. - public static final double kTurnCurveExponent = 4.0; + // Max acceleration per second for the turn limiter. + public static final double kTurnMaxRatePerSec = 8.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.5; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 693612c..c2d88b4 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,7 +43,7 @@ public RobotContainer() { private void configureBindings() { drivetrain.setDefaultCommand( drivetrain - .arcadeDrive(m_driverController::getLeftX, m_driverController::getLeftY) + .arcadeDrive(m_driverController::getRightX, m_driverController::getLeftY) .beforeStarting(drivetrain::resetAccelerationLimiters)); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 5e26cc0..c263f4a 100755 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -70,8 +70,11 @@ public Drivetrain() { public Command arcadeDrive(DoubleSupplier x, DoubleSupplier y) { return run( () -> { - double forward = driveLimiter.calculate(y.getAsDouble()); - double turn = turnLimiter.calculate(x.getAsDouble()); + double limitedDrive = driveLimiter.calculate(y.getAsDouble()); + double limitedTurn = turnLimiter.calculate(x.getAsDouble()); + + double forward = Math.clamp(limitedDrive, -Constants.DrivetrainConstants.kDriveMaxPower, Constants.DrivetrainConstants.kDriveMaxPower); + double turn = Math.clamp(limitedTurn, -Constants.DrivetrainConstants.kTurnMaxPower, Constants.DrivetrainConstants.kTurnMaxPower); differentialDrive.arcadeDrive(forward, turn); }); } From bebfdb3693f1029b4319cd8a6fb6e43aa117012e Mon Sep 17 00:00:00 2001 From: Aarav Sharma Date: Wed, 1 Jul 2026 14:16:42 -0600 Subject: [PATCH 6/8] fix(drivetrain): replace Math.clamp with inline min/max for Java 17 Math.clamp was added in Java 21 but this project targets Java 17 (build.gradle:9-10). --- src/main/java/frc/robot/subsystems/Drivetrain.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index c263f4a..a0ad4a3 100755 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -73,8 +73,8 @@ public Command arcadeDrive(DoubleSupplier x, DoubleSupplier y) { double limitedDrive = driveLimiter.calculate(y.getAsDouble()); double limitedTurn = turnLimiter.calculate(x.getAsDouble()); - double forward = Math.clamp(limitedDrive, -Constants.DrivetrainConstants.kDriveMaxPower, Constants.DrivetrainConstants.kDriveMaxPower); - double turn = Math.clamp(limitedTurn, -Constants.DrivetrainConstants.kTurnMaxPower, Constants.DrivetrainConstants.kTurnMaxPower); + 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); }); } From 6c99bfab7dd6440910197bff71cecd123c0e1731 Mon Sep 17 00:00:00 2001 From: Aarav Sharma Date: Wed, 1 Jul 2026 19:37:57 -0600 Subject: [PATCH 7/8] chore(drivetrain): increase drive and turn rate and power limits --- src/main/java/frc/robot/Constants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9ae336b..f1f43fa 100755 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,21 +16,21 @@ public static class DrivetrainConstants { public static final int BackRightId = 13; // Max acceleration per second for the drive limiter. - public static final double kDriveMaxRatePerSec = 8.0; + 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.5; + public static final double kDriveMaxPower = 0.8; // Max acceleration per second for the turn limiter. - public static final double kTurnMaxRatePerSec = 8.0; - + 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.5; + public static final double kTurnMaxPower = 0.8; } } From f2f01fc9065b07d7c800dceecd54a4b867df6ecc Mon Sep 17 00:00:00 2001 From: Aarav Sharma Date: Wed, 1 Jul 2026 19:40:51 -0600 Subject: [PATCH 8/8] style(drivetrain): wrap clamp expressions in arcadeDrive --- src/main/java/frc/robot/subsystems/Drivetrain.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index a0ad4a3..858e38e 100755 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -73,8 +73,14 @@ public Command arcadeDrive(DoubleSupplier x, DoubleSupplier y) { 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)); + 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); }); }