From a3ff669c5f209e63b0e7c42d91d9e00affac0160 Mon Sep 17 00:00:00 2001 From: Programmers Date: Sun, 21 Apr 2024 15:46:13 -0500 Subject: [PATCH] Cap max speeds for driving. --- .../frc/robot/commands/movement/SwerveCommand.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/movement/SwerveCommand.java b/src/main/java/frc/robot/commands/movement/SwerveCommand.java index 38043f5..e6d50b5 100644 --- a/src/main/java/frc/robot/commands/movement/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/movement/SwerveCommand.java @@ -1,6 +1,8 @@ package frc.robot.commands.movement; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; @@ -23,6 +25,8 @@ */ public class SwerveCommand extends Command { + static private final double MAX_SPEED_TRANSLATION = 2.0; + private final PIDController THETA_CONTROLLER; private final IntSupplier POV_DEGREE; private boolean isCardinalLocking = false; @@ -109,13 +113,13 @@ public void execute() { // DRIVETRAIN.getPose().getRotation().getDegrees(), THETA_CONTROLLER.getSetpoint()); // } - double rotationalVelocity = rotationPercentage; + double rotationalVelocity = MathUtil.clamp(rotationPercentage, -Math.PI, Math.PI); DRIVETRAIN.setControl( DRIVE_REQUEST - .withVelocityX(xPercentage) // Drive forward with + .withVelocityX(MathUtil.clamp(xPercentage, -MAX_SPEED_TRANSLATION, MAX_SPEED_TRANSLATION)) // Drive forward with // negative Y (forward) - .withVelocityY(yPercentage) // Drive left with negative X (left) + .withVelocityY(MathUtil.clamp(yPercentage, -MAX_SPEED_TRANSLATION, MAX_SPEED_TRANSLATION)) // Drive left with negative X (left) .withRotationalRate(rotationalVelocity)); }