From 9b657dd4ada8368a572fc953b8f3b1cf769d8f8d Mon Sep 17 00:00:00 2001 From: BlueZeeKing Date: Tue, 2 Jan 2024 18:20:56 -0500 Subject: [PATCH 1/5] Add new ratelimiting --- .../java/frc/constants/DriveConstants.java | 6 +- .../frc/robot/subsystems/DriveSubsystem.java | 129 ++++++------------ .../java/frc/utils/CustomSlewRateLimiter.java | 28 ++++ src/main/java/frc/utils/SwerveUtils.java | 26 ++++ 4 files changed, 101 insertions(+), 88 deletions(-) create mode 100644 src/main/java/frc/utils/CustomSlewRateLimiter.java diff --git a/src/main/java/frc/constants/DriveConstants.java b/src/main/java/frc/constants/DriveConstants.java index 4894ea9..f99a67a 100644 --- a/src/main/java/frc/constants/DriveConstants.java +++ b/src/main/java/frc/constants/DriveConstants.java @@ -14,9 +14,9 @@ public class DriveConstants { public static final double MAX_SPEED_METERS_PER_SECOND = 4.8; public static final double MAX_ANGULAR_SPEED = 2 * Math.PI; // radians per second - public static final double DIRECTION_SLEW_RATE = 1.2; // radians per second - public static final double MAGNITUDE_SLEW_RATE = 1.8; // percent per second (1 = 100%) - public static final double ROTATIONAL_SLEW_RATE = 2.0; // percent per second (1 = 100%) + public static final double MAX_ACCELERATION = 1; // meters per second per second + public static final double MAX_DRIVE_ANGULAR_VELOCITY = 1; // radians per second + public static final double MAX_ROTATIONAL_ACCELERATION = 2; // percent per second // Chassis configuration public static final double TRACK_WIDTH_METERS = Units.inchesToMeters(27.5); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index da319cf..5593b7d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -9,11 +9,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.*; -import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.constants.CANMappings; import frc.constants.DriveConstants; +import frc.utils.CustomSlewRateLimiter; import frc.utils.SwerveUtils; public class DriveSubsystem extends SubsystemBase { @@ -45,20 +45,23 @@ public class DriveSubsystem extends SubsystemBase { // The gyro sensor private final AHRS gyro = new AHRS(); - // Slew rate filter variables for controlling lateral acceleration private double currentRotation = 0.0; - private double currentTranslationDir = 0.0; - private double currentTranslationMag = 0.0; - private SlewRateLimiter magLimiter = new SlewRateLimiter(DriveConstants.MAGNITUDE_SLEW_RATE); - private SlewRateLimiter rotLimiter = new SlewRateLimiter(DriveConstants.ROTATIONAL_SLEW_RATE); - private double prevTime = WPIUtilJNI.now() * 1e-6; + // Slew rate filter variables for controlling lateral acceleration + private double currentTranslationDir = 0.0; // radian + private double currentTranslationMag = 0.0; // meter per second + private SlewRateLimiter magLimiter = new SlewRateLimiter(DriveConstants.MAX_ACCELERATION); + private CustomSlewRateLimiter dirLimiter = + new CustomSlewRateLimiter(DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY, 0); + + private SlewRateLimiter rotLimiter = + new SlewRateLimiter(DriveConstants.MAX_ROTATIONAL_ACCELERATION); // Odometry class for tracking robot pose SwerveDriveOdometry odometry = new SwerveDriveOdometry( DriveConstants.DRIVE_KINEMATICS, - Rotation2d.fromDegrees(this.gyro.getAngle()), + getHeading(), new SwerveModulePosition[] { this.frontLeft.getPosition(), this.frontRight.getPosition(), @@ -123,7 +126,8 @@ public void resetOdometry(Pose2d pose) { } /** - * Method to drive the robot using joystick info. + * Method to drive the robot using joystick info. This method may not work as intended if the + * joystick does not move in a circle * * @param xSpeed Speed of the robot in the x direction (forward). * @param ySpeed Speed of the robot in the y direction (sideways). @@ -139,95 +143,50 @@ public void drive( if (rateLimit) { // Convert XY to polar(theta and magnitude) for rate limiting - double inputTranslationDir = Math.atan2(ySpeed, xSpeed); - double inputTranslationMag = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); - - // Calculate the direction slew rate based on an estimate of the lateral acceleration - // if driving set the rotation slew rate to a constant divided by current translation speed so - // the faster you are, the slower you turn - // else practically don't limit rotational speed - double directionSlewRate; - if (this.currentTranslationMag != 0.0) { - directionSlewRate = - Math.abs(DriveConstants.DIRECTION_SLEW_RATE / this.currentTranslationMag); - } else { - directionSlewRate = - 500.0; // some high number that means the slew rate is effectively instantaneous + double inputTranslationDir = SwerveUtils.wrapAngle(Math.atan2(ySpeed, xSpeed)); + double inputTranslationMag = + Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)) + * DriveConstants.MAX_SPEED_METERS_PER_SECOND; + + if (currentTranslationMag < 0.1) { + currentTranslationDir = inputTranslationDir; + } else if (inputTranslationMag < 0.1) { + inputTranslationDir = currentTranslationDir; } - double currentTime = WPIUtilJNI.now() * 1e-6; - double elapsedTime = currentTime - this.prevTime; - double angleDif = - SwerveUtils.angleDifference(inputTranslationDir, this.currentTranslationDir); - - /* - * Description of the if below - * - * if the difference between the target direction and current direction is smallish, - * step the current direction slowly to match the input based on the slew rate and time. - * Allow the translation speed to change based on the limiter. This will result in the robot turning while it maintains speed - * - * if the difference is large and the robot is moving, slow down - * if stopped swap the direction and accelerate away. - * This performs a 180 flip that is then corrected by the first case. The robot stops, then accelerates backward - * - * if the difference is in the middle, step the current angle towards the target and slow the robot. - * This will eventually switch to the first case when the robot accelerates again. - */ - - // if angle is less than 81 degrees, step the current translation direction to the input with - // a step size the size of the slew rate, this will prevent the dir from changing too fast - // then limit the translation speed based on the limiter - if (angleDif < 0.45 * Math.PI) { - this.currentTranslationDir = - SwerveUtils.stepTowardsCircular( - currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); - this.currentTranslationMag = magLimiter.calculate(inputTranslationMag); - } else if (angleDif > 0.85 * Math.PI) { - // if the angle is greater than 153 degrees and the requested translation speed is non-zero - // slow the robot speed to zero - if (this.currentTranslationMag - > 1e-4) { // some small number to avoid floating-point errors with equality checking - // keep currentTranslationDir unchanged - this.currentTranslationMag = magLimiter.calculate(0.0); - } else { - // if the robot speed is already at zero flip the angle so the robot reverses direction - // and begin to re-accelerate - this.currentTranslationDir = SwerveUtils.wrapAngle(currentTranslationDir + Math.PI); - this.currentTranslationMag = magLimiter.calculate(inputTranslationMag); - } - } else { - // if the angle is between the two checks, slow the robot to zero and begin to turn - this.currentTranslationDir = - SwerveUtils.stepTowardsCircular( - currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); - this.currentTranslationMag = magLimiter.calculate(0.0); + inputTranslationDir = SwerveUtils.optimizeAngle(inputTranslationDir, currentTranslationDir); + + if (Math.abs(inputTranslationDir - currentTranslationDir) > Math.PI / 2.0) { + inputTranslationDir += Math.PI; + inputTranslationMag *= -1; } - this.prevTime = currentTime; + + inputTranslationDir = SwerveUtils.optimizeAngle(inputTranslationDir, currentTranslationDir); + + inputTranslationMag *= 1 - Math.abs(inputTranslationDir - currentTranslationDir) / Math.PI; + + dirLimiter.setRateLimit( + DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY + * (1 - currentTranslationMag / DriveConstants.MAX_SPEED_METERS_PER_SECOND)); + + this.currentTranslationMag = magLimiter.calculate(inputTranslationMag); + this.currentTranslationDir = dirLimiter.calculate(inputTranslationDir); xSpeedCommanded = this.currentTranslationMag * Math.cos(currentTranslationDir); ySpeedCommanded = this.currentTranslationMag * Math.sin(currentTranslationDir); - this.currentRotation = rotLimiter.calculate(rot); + this.currentRotation = rotLimiter.calculate(rot) * DriveConstants.MAX_ANGULAR_SPEED; } else { - xSpeedCommanded = xSpeed; - ySpeedCommanded = ySpeed; - this.currentRotation = rot; + xSpeedCommanded = xSpeed * DriveConstants.MAX_SPEED_METERS_PER_SECOND; + ySpeedCommanded = ySpeed * DriveConstants.MAX_SPEED_METERS_PER_SECOND; + this.currentRotation = rot * DriveConstants.MAX_ANGULAR_SPEED; } - // Convert the commanded speeds into the correct units for the drivetrain - double xSpeedDelivered = xSpeedCommanded * DriveConstants.MAX_SPEED_METERS_PER_SECOND; - double ySpeedDelivered = ySpeedCommanded * DriveConstants.MAX_SPEED_METERS_PER_SECOND; - double rotDelivered = this.currentRotation * DriveConstants.MAX_ANGULAR_SPEED; - var swerveModuleStates = DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeedDelivered, - ySpeedDelivered, - rotDelivered, - Rotation2d.fromDegrees(this.gyro.getAngle())) - : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); + xSpeedCommanded, ySpeedCommanded, this.currentRotation, getHeading()) + : new ChassisSpeeds(xSpeedCommanded, ySpeedCommanded, this.currentRotation)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND); this.frontLeft.setDesiredState(swerveModuleStates[0]); diff --git a/src/main/java/frc/utils/CustomSlewRateLimiter.java b/src/main/java/frc/utils/CustomSlewRateLimiter.java new file mode 100644 index 0000000..02b27c6 --- /dev/null +++ b/src/main/java/frc/utils/CustomSlewRateLimiter.java @@ -0,0 +1,28 @@ +package frc.utils; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUtil; + +public class CustomSlewRateLimiter { + private double rateLimit; + private double prevVal; + private double prevTime; + + public CustomSlewRateLimiter(double rateLimit, double initialVal) { + this.rateLimit = rateLimit; + this.prevVal = initialVal; + this.prevTime = MathSharedStore.getTimestamp(); + } + + public void setRateLimit(double rateLimit) { + this.rateLimit = rateLimit; + } + + public double calculate(double input) { + double currentTime = MathSharedStore.getTimestamp(); + double elapsedTime = currentTime - prevTime; + prevVal += MathUtil.clamp(input - prevVal, -rateLimit * elapsedTime, rateLimit * elapsedTime); + prevTime = currentTime; + return prevVal; + } +} diff --git a/src/main/java/frc/utils/SwerveUtils.java b/src/main/java/frc/utils/SwerveUtils.java index 9d0221d..36fac63 100644 --- a/src/main/java/frc/utils/SwerveUtils.java +++ b/src/main/java/frc/utils/SwerveUtils.java @@ -1,6 +1,32 @@ package frc.utils; public class SwerveUtils { + /** + * Take two angles in radians and find the combonation with the shortest distance + * + * @param self this angle + * @param other the other comparison angle + * @return An equivelent angle to self + */ + public static double optimizeAngle(double self, double other) { + double a = wrapAngle(other); + double b = wrapAngle(self); + + double b1 = b + 2.0 * Math.PI; + double b2 = b - 2.0 * Math.PI; + + double diff = Math.abs(a - b); + double diff1 = Math.abs(a - b1); + double diff2 = Math.abs(a - b2); + + if (diff < diff1 && diff < diff2) { + return b; + } else if (diff1 < diff && diff1 < diff2) { + return b1; + } else { + return b2; + } + } /** * Steps a value towards a target with a specified step size. From 44e08986492e698f03065ad73fe32eabaaa9940d Mon Sep 17 00:00:00 2001 From: BlueZeeKing Date: Tue, 2 Jan 2024 18:30:56 -0500 Subject: [PATCH 2/5] Fix amount divided by --- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 5593b7d..2e48a31 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -163,7 +163,8 @@ public void drive( inputTranslationDir = SwerveUtils.optimizeAngle(inputTranslationDir, currentTranslationDir); - inputTranslationMag *= 1 - Math.abs(inputTranslationDir - currentTranslationDir) / Math.PI; + inputTranslationMag *= + 1 - Math.abs(inputTranslationDir - currentTranslationDir) / (Math.PI / 2); dirLimiter.setRateLimit( DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY From 0fd202d2adff71e053ab0ea795f6fa9064eacf29 Mon Sep 17 00:00:00 2001 From: BlueZeeKing Date: Wed, 3 Jan 2024 21:11:02 -0500 Subject: [PATCH 3/5] Change algorithm Switch to using preexisting swerve util functions --- .../java/frc/constants/DriveConstants.java | 5 ++- .../frc/robot/subsystems/DriveSubsystem.java | 38 ++++++++++++------- .../java/frc/utils/CustomSlewRateLimiter.java | 28 -------------- src/main/java/frc/utils/SwerveUtils.java | 27 ------------- 4 files changed, 27 insertions(+), 71 deletions(-) delete mode 100644 src/main/java/frc/utils/CustomSlewRateLimiter.java diff --git a/src/main/java/frc/constants/DriveConstants.java b/src/main/java/frc/constants/DriveConstants.java index f99a67a..a5fd1d5 100644 --- a/src/main/java/frc/constants/DriveConstants.java +++ b/src/main/java/frc/constants/DriveConstants.java @@ -14,8 +14,9 @@ public class DriveConstants { public static final double MAX_SPEED_METERS_PER_SECOND = 4.8; public static final double MAX_ANGULAR_SPEED = 2 * Math.PI; // radians per second - public static final double MAX_ACCELERATION = 1; // meters per second per second - public static final double MAX_DRIVE_ANGULAR_VELOCITY = 1; // radians per second + // TODO: Tune these values + public static final double MAX_ACCELERATION = 9.6; // meters per second per second + public static final double MAX_DRIVE_ANGULAR_VELOCITY = 8; // radians per second public static final double MAX_ROTATIONAL_ACCELERATION = 2; // percent per second // Chassis configuration diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 2e48a31..fb3569d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -13,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.constants.CANMappings; import frc.constants.DriveConstants; -import frc.utils.CustomSlewRateLimiter; import frc.utils.SwerveUtils; public class DriveSubsystem extends SubsystemBase { @@ -51,8 +51,8 @@ public class DriveSubsystem extends SubsystemBase { private double currentTranslationDir = 0.0; // radian private double currentTranslationMag = 0.0; // meter per second private SlewRateLimiter magLimiter = new SlewRateLimiter(DriveConstants.MAX_ACCELERATION); - private CustomSlewRateLimiter dirLimiter = - new CustomSlewRateLimiter(DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY, 0); + + private double prevTime = MathSharedStore.getTimestamp(); private SlewRateLimiter rotLimiter = new SlewRateLimiter(DriveConstants.MAX_ROTATIONAL_ACCELERATION); @@ -145,8 +145,10 @@ public void drive( // Convert XY to polar(theta and magnitude) for rate limiting double inputTranslationDir = SwerveUtils.wrapAngle(Math.atan2(ySpeed, xSpeed)); double inputTranslationMag = - Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)) - * DriveConstants.MAX_SPEED_METERS_PER_SECOND; + Math.min( + Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)) + * DriveConstants.MAX_SPEED_METERS_PER_SECOND, + DriveConstants.MAX_SPEED_METERS_PER_SECOND); if (currentTranslationMag < 0.1) { currentTranslationDir = inputTranslationDir; @@ -154,25 +156,33 @@ public void drive( inputTranslationDir = currentTranslationDir; } - inputTranslationDir = SwerveUtils.optimizeAngle(inputTranslationDir, currentTranslationDir); - - if (Math.abs(inputTranslationDir - currentTranslationDir) > Math.PI / 2.0) { + if (SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) > Math.PI / 2.0) { inputTranslationDir += Math.PI; inputTranslationMag *= -1; } - inputTranslationDir = SwerveUtils.optimizeAngle(inputTranslationDir, currentTranslationDir); + inputTranslationDir = SwerveUtils.wrapAngle(inputTranslationDir); inputTranslationMag *= - 1 - Math.abs(inputTranslationDir - currentTranslationDir) / (Math.PI / 2); + 1 + - SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) + / (Math.PI / 2); + + double dirLimit = + Math.max( + 0, + DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY + * ((1 - currentTranslationMag / DriveConstants.MAX_SPEED_METERS_PER_SECOND) * 0.90 + + 0.10)); - dirLimiter.setRateLimit( - DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY - * (1 - currentTranslationMag / DriveConstants.MAX_SPEED_METERS_PER_SECOND)); + double currentTime = MathSharedStore.getTimestamp(); this.currentTranslationMag = magLimiter.calculate(inputTranslationMag); - this.currentTranslationDir = dirLimiter.calculate(inputTranslationDir); + this.currentTranslationDir = + SwerveUtils.stepTowardsCircular( + currentTranslationDir, inputTranslationDir, dirLimit * (currentTime - prevTime)); + prevTime = currentTime; xSpeedCommanded = this.currentTranslationMag * Math.cos(currentTranslationDir); ySpeedCommanded = this.currentTranslationMag * Math.sin(currentTranslationDir); this.currentRotation = rotLimiter.calculate(rot) * DriveConstants.MAX_ANGULAR_SPEED; diff --git a/src/main/java/frc/utils/CustomSlewRateLimiter.java b/src/main/java/frc/utils/CustomSlewRateLimiter.java deleted file mode 100644 index 02b27c6..0000000 --- a/src/main/java/frc/utils/CustomSlewRateLimiter.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.utils; - -import edu.wpi.first.math.MathSharedStore; -import edu.wpi.first.math.MathUtil; - -public class CustomSlewRateLimiter { - private double rateLimit; - private double prevVal; - private double prevTime; - - public CustomSlewRateLimiter(double rateLimit, double initialVal) { - this.rateLimit = rateLimit; - this.prevVal = initialVal; - this.prevTime = MathSharedStore.getTimestamp(); - } - - public void setRateLimit(double rateLimit) { - this.rateLimit = rateLimit; - } - - public double calculate(double input) { - double currentTime = MathSharedStore.getTimestamp(); - double elapsedTime = currentTime - prevTime; - prevVal += MathUtil.clamp(input - prevVal, -rateLimit * elapsedTime, rateLimit * elapsedTime); - prevTime = currentTime; - return prevVal; - } -} diff --git a/src/main/java/frc/utils/SwerveUtils.java b/src/main/java/frc/utils/SwerveUtils.java index 36fac63..5f32b67 100644 --- a/src/main/java/frc/utils/SwerveUtils.java +++ b/src/main/java/frc/utils/SwerveUtils.java @@ -1,33 +1,6 @@ package frc.utils; public class SwerveUtils { - /** - * Take two angles in radians and find the combonation with the shortest distance - * - * @param self this angle - * @param other the other comparison angle - * @return An equivelent angle to self - */ - public static double optimizeAngle(double self, double other) { - double a = wrapAngle(other); - double b = wrapAngle(self); - - double b1 = b + 2.0 * Math.PI; - double b2 = b - 2.0 * Math.PI; - - double diff = Math.abs(a - b); - double diff1 = Math.abs(a - b1); - double diff2 = Math.abs(a - b2); - - if (diff < diff1 && diff < diff2) { - return b; - } else if (diff1 < diff && diff1 < diff2) { - return b1; - } else { - return b2; - } - } - /** * Steps a value towards a target with a specified step size. * From 30ada95ffd5f101bd8127ae478dc520047f0913c Mon Sep 17 00:00:00 2001 From: BlueZeeKing Date: Wed, 3 Jan 2024 21:31:54 -0500 Subject: [PATCH 4/5] Fix the robot turning weird when angle switch > 90 deg --- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index fb3569d..a676e9e 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -157,7 +157,7 @@ public void drive( } if (SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) > Math.PI / 2.0) { - inputTranslationDir += Math.PI; + inputTranslationDir = currentTranslationDir; inputTranslationMag *= -1; } From 677faa7f85292e0f7304dba5a176689540f93946 Mon Sep 17 00:00:00 2001 From: BlueZeeKing Date: Wed, 3 Jan 2024 23:30:54 -0500 Subject: [PATCH 5/5] Add some comments --- .../java/frc/robot/subsystems/DriveSubsystem.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index a676e9e..039b0cb 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -150,24 +150,32 @@ public void drive( * DriveConstants.MAX_SPEED_METERS_PER_SECOND, DriveConstants.MAX_SPEED_METERS_PER_SECOND); + // If either translation magnitudes are low enough, it's direction is irrelevant if (currentTranslationMag < 0.1) { currentTranslationDir = inputTranslationDir; } else if (inputTranslationMag < 0.1) { inputTranslationDir = currentTranslationDir; } + // If the new direction is over 90º from the current direction, slow the robot + // to a near stop before continuing. The block above activates when the robot + // has slowed down enough if (SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) > Math.PI / 2.0) { inputTranslationDir = currentTranslationDir; inputTranslationMag *= -1; } - inputTranslationDir = SwerveUtils.wrapAngle(inputTranslationDir); - + // Decrease the requested speed based on how far the current translation + // direction is from the requested one. A 90º angle means the requested + // magnitude is 0, a 0º angle means the magnitude is unchanged. inputTranslationMag *= 1 - SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) / (Math.PI / 2); + // Set the direction limit to a percent of the maxiumum based on the percent of + // max speed we are currently going at. This is limited to prevent fully + // disabling steering and leaves it at 10%. double dirLimit = Math.max( 0, @@ -198,8 +206,10 @@ public void drive( ? ChassisSpeeds.fromFieldRelativeSpeeds( xSpeedCommanded, ySpeedCommanded, this.currentRotation, getHeading()) : new ChassisSpeeds(xSpeedCommanded, ySpeedCommanded, this.currentRotation)); + SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND); + this.frontLeft.setDesiredState(swerveModuleStates[0]); this.frontRight.setDesiredState(swerveModuleStates[1]); this.rearLeft.setDesiredState(swerveModuleStates[2]);