Skip to content

limit scoring drive acceleration #75

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
vivi-o wants to merge 3 commits into
base: main
Choose a base branch
from
Open

limit scoring drive acceleration #75

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
810da77
use rate limiter to cap drive acceleration
vivi-o Mar 24, 2026
1c0774d
define limiter outside of loop so it actually works
vivi-o Mar 24, 2026
0c2ec98
works in sim
vivi-o Mar 24, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 19 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -182,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(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 =
12.1; // TODO 11.8 for practice batteries and 12.2 for comp batteries. maybe also do leds?
Expand Down Expand Up @@ -575,6 +580,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(
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));

Expand Down
Loading