diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c0510ac..f1f43fa 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; @@ -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; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8e14214..c2d88b4 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::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 0c3634d..858e38e 100755 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/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 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; + } +}