Add new ratelimiting

src/main/java/frc/constants/DriveConstants.java

@@ -14,9 +14,10 @@ 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%)
+  // 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
   public static final double TRACK_WIDTH_METERS = Units.inchesToMeters(27.5);

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -5,11 +5,11 @@
 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;
 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;
@@ -45,14 +45,17 @@ 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 double prevTime = MathSharedStore.getTimestamp();
+
+  private SlewRateLimiter rotLimiter =
+      new SlewRateLimiter(DriveConstants.MAX_ROTATIONAL_ACCELERATION);
 
   // Odometry class for tracking robot pose
   SwerveDriveOdometry odometry =
@@ -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,94 +143,73 @@ 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.min(
+              Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2))
+                  * 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;
       }
 
-      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);
+      // 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;
       }
-      this.prevTime = currentTime;
 
+      // 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,
+              DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY
+                  * ((1 - currentTranslationMag / DriveConstants.MAX_SPEED_METERS_PER_SECOND) * 0.90
+                      + 0.10));
+
+      double currentTime = MathSharedStore.getTimestamp();
+
+      this.currentTranslationMag = magLimiter.calculate(inputTranslationMag);
+      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);
+      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, getHeading())
-                : 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]);
     this.frontRight.setDesiredState(swerveModuleStates[1]);
     this.rearLeft.setDesiredState(swerveModuleStates[2]);

src/main/java/frc/utils/SwerveUtils.java

@@ -1,7 +1,6 @@
 package frc.utils;
 
 public class SwerveUtils {
-
   /**
    * Steps a value towards a target with a specified step size.
    *