From 810da77781172ceea51c2a9aa017cc53bc8e8eb2 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Mon, 23 Mar 2026 23:28:42 -0700 Subject: [PATCH 1/3] use rate limiter to cap drive acceleration --- src/main/java/frc/robot/Robot.java | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 33af4858..92c27349 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import com.playingwithfusion.BattFuelGauge; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -575,6 +576,20 @@ public Robot() { Commands.runOnce(() -> addAutos()) .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); + new Trigger(() -> Superstructure.getState().isAScoreState()) + .whileTrue( + swerve + .driveOpenLoopFieldRelative( + () -> + new ChassisSpeeds( + new SlewRateLimiter(5.0).calculate(modifyJoystick(driver.getLeftY())) + * (SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()), + new SlewRateLimiter(5.0).calculate(modifyJoystick(driver.getLeftX())) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + new SlewRateLimiter(5.0).calculate(modifyJoystick(driver.getRightX())) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed()) + .times(-1)) + .withName("default")); SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); From 1c0774d8e3626287aff58675f42757499b741386 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Mon, 23 Mar 2026 23:42:42 -0700 Subject: [PATCH 2/3] define limiter outside of loop so it actually works --- src/main/java/frc/robot/Robot.java | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 92c27349..d484c14a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -183,6 +183,10 @@ public enum RobotEdition { private static final double CAN_ERROR_TIME_THRESHOLD = 0.5; // Seconds to disable alert private static final double CANIVORE_ERROR_TIME_THRESHOLD = 0.5; + private final SlewRateLimiter xAccelLimiter = new SlewRateLimiter(10.0); + private final SlewRateLimiter yAccelLimiter = new SlewRateLimiter(10.0); + private final SlewRateLimiter rAccelLimiter = new SlewRateLimiter(10.0); + private static int lowBatteryCycleCount = 0; private static final double lowBatteryVoltage = 12.1; // TODO 11.8 for practice batteries and 12.2 for comp batteries. maybe also do leds? @@ -582,13 +586,13 @@ public Robot() { .driveOpenLoopFieldRelative( () -> new ChassisSpeeds( - new SlewRateLimiter(5.0).calculate(modifyJoystick(driver.getLeftY())) - * (SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()), - new SlewRateLimiter(5.0).calculate(modifyJoystick(driver.getLeftX())) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - new SlewRateLimiter(5.0).calculate(modifyJoystick(driver.getRightX())) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed()) - .times(-1)) + yAccelLimiter.calculate(modifyJoystick(driver.getLeftY())) + * (SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()), + xAccelLimiter.calculate(modifyJoystick(driver.getLeftX())) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + rAccelLimiter.calculate(modifyJoystick(driver.getRightX())) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed()) + .times(-1)) .withName("default")); SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); From 0c2ec9891c510a3ae232435932a0cb3d6c0e3738 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Mon, 23 Mar 2026 23:45:36 -0700 Subject: [PATCH 3/3] works in sim --- src/main/java/frc/robot/Robot.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d484c14a..24384f92 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -183,9 +183,9 @@ public enum RobotEdition { private static final double CAN_ERROR_TIME_THRESHOLD = 0.5; // Seconds to disable alert private static final double CANIVORE_ERROR_TIME_THRESHOLD = 0.5; - private final SlewRateLimiter xAccelLimiter = new SlewRateLimiter(10.0); - private final SlewRateLimiter yAccelLimiter = new SlewRateLimiter(10.0); - private final SlewRateLimiter rAccelLimiter = new SlewRateLimiter(10.0); + private final SlewRateLimiter xAccelLimiter = new SlewRateLimiter(3.0); + private final SlewRateLimiter yAccelLimiter = new SlewRateLimiter(3.0); + private final SlewRateLimiter rAccelLimiter = new SlewRateLimiter(3.0); private static int lowBatteryCycleCount = 0; private static final double lowBatteryVoltage =