Fix slew rate limiter drift when stopping

src/main/java/frc/robot/Constants.java

@@ -30,9 +30,8 @@ public static final class DriveConstants {
     public static final double kMaxSpeedMetersPerSecond = 4.8;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
-    public static final double kDirectionSlewRate = 1.2; // radians per second
-    public static final double kMagnitudeSlewRate = 1.8; // percent per second (1 = 100%)
-    public static final double kRotationalSlewRate = 2.0; // percent per second (1 = 100%)
+    public static final double kMagnitudeSlewRate = 1.8 * kMaxSpeedMetersPerSecond; // meters per second^2
+    public static final double kRotationalSlewRate = 2.0 * kMaxAngularSpeed;        // radians per second^2
 
     // Chassis configuration
     public static final double kTrackWidth = Units.inchesToMeters(26.5);

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

@@ -4,7 +4,6 @@
 
 package frc.robot.subsystems;
 
-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.ChassisSpeeds;
@@ -44,14 +43,8 @@ public class DriveSubsystem extends SubsystemBase {
   // The gyro sensor
   private final ADIS16470_IMU m_gyro = new ADIS16470_IMU();
 
-  // Slew rate filter variables for controlling lateral acceleration
-  private double m_currentRotation = 0.0;
-  private double m_currentTranslationDir = 0.0;
-  private double m_currentTranslationMag = 0.0;
-
-  private SlewRateLimiter m_magLimiter = new SlewRateLimiter(DriveConstants.kMagnitudeSlewRate);
-  private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(DriveConstants.kRotationalSlewRate);
   private double m_prevTime = WPIUtilJNI.now() * 1e-6;
+  private ChassisSpeeds m_prevTarget = new ChassisSpeeds();
 
   // Odometry class for tracking robot pose
   SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
@@ -118,73 +111,38 @@ public void resetOdometry(Pose2d pose) {
    * @param rateLimit     Whether to enable rate limiting for smoother control.
    */
   public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) {
-
-    double xSpeedCommanded;
-    double ySpeedCommanded;
-
-    if (rateLimit) {
-      // Convert XY to polar 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
-      double directionSlewRate;
-      if (m_currentTranslationMag != 0.0) {
-        directionSlewRate = Math.abs(DriveConstants.kDirectionSlewRate / m_currentTranslationMag);
-      } else {
-        directionSlewRate = 500.0; //some high number that means the slew rate is effectively instantaneous
-      }
-
-
-      double currentTime = WPIUtilJNI.now() * 1e-6;
-      double elapsedTime = currentTime - m_prevTime;
-      double angleDif = SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDir);
-      if (angleDif < 0.45*Math.PI) {
-        m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime);
-        m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag);
-      }
-      else if (angleDif > 0.85*Math.PI) {
-        if (m_currentTranslationMag > 1e-4) { //some small number to avoid floating-point errors with equality checking
-          // keep currentTranslationDir unchanged
-          m_currentTranslationMag = m_magLimiter.calculate(0.0);
-        }
-        else {
-          m_currentTranslationDir = SwerveUtils.WrapAngle(m_currentTranslationDir + Math.PI);
-          m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag);
-        }
-      }
-      else {
-        m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime);
-        m_currentTranslationMag = m_magLimiter.calculate(0.0);
-      }
-      m_prevTime = currentTime;
-
-      xSpeedCommanded = m_currentTranslationMag * Math.cos(m_currentTranslationDir);
-      ySpeedCommanded = m_currentTranslationMag * Math.sin(m_currentTranslationDir);
-      m_currentRotation = m_rotLimiter.calculate(rot);
-
 
-    } else {
-      xSpeedCommanded = xSpeed;
-      ySpeedCommanded = ySpeed;
-      m_currentRotation = rot;
+    // Convert the commanded speeds into the correct units for the drivetrain
+    xSpeed *= DriveConstants.kMaxSpeedMetersPerSecond;
+    ySpeed *= DriveConstants.kMaxSpeedMetersPerSecond;
+    rot *= DriveConstants.kMaxAngularSpeed;
+
+    // Get the target chassis speeds relative to the robot
+    final ChassisSpeeds vel = ( fieldRelative ?
+      ChassisSpeeds.fromFieldRelativeSpeeds( xSpeed, ySpeed, rot, Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)) )
+        : new ChassisSpeeds( xSpeed, ySpeed, rot )
+    );
+
+    // Rate limit if applicable
+    if(rateLimit) {
+      final double
+        currentTime = WPIUtilJNI.now() * 1e-6,
+        elapsedTime = currentTime - m_prevTime;
+      SwerveUtils.RateLimitVelocity(
+        vel, m_prevTarget, elapsedTime,
+        DriveConstants.kMagnitudeSlewRate, DriveConstants.kRotationalSlewRate);
+      m_prevTime = currentTime;
+      m_prevTarget = vel;
     }
 
-    // Convert the commanded speeds into the correct units for the drivetrain
-    double xSpeedDelivered = xSpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond;
-    double ySpeedDelivered = ySpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond;
-    double rotDelivered = m_currentRotation * DriveConstants.kMaxAngularSpeed;
-
-    var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
-        fieldRelative
-            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
-            : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
-    SwerveDriveKinematics.desaturateWheelSpeeds(
-        swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
+    var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(vel);
+    SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
+
     m_frontLeft.setDesiredState(swerveModuleStates[0]);
     m_frontRight.setDesiredState(swerveModuleStates[1]);
     m_rearLeft.setDesiredState(swerveModuleStates[2]);
     m_rearRight.setDesiredState(swerveModuleStates[3]);
+
   }
 
   /**

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

@@ -1,89 +1,34 @@
 package frc.utils;
 
-public class SwerveUtils {
-
-    /**
-     * Steps a value towards a target with a specified step size.
-     * @param _current The current or starting value.  Can be positive or negative.
-     * @param _target The target value the algorithm will step towards.  Can be positive or negative.
-     * @param _stepsize The maximum step size that can be taken.
-     * @return The new value for {@code _current} after performing the specified step towards the specified target.
-     */
-    public static double StepTowards(double _current, double _target, double _stepsize) {
-        if (Math.abs(_current - _target) <= _stepsize) {
-            return _target;
-        }
-        else if (_target < _current) {
-            return _current - _stepsize;
-        }
-        else {
-            return _current + _stepsize;
-        }
-    }
-
-    /**
-     * Steps a value (angle) towards a target (angle) taking the shortest path with a specified step size.
-     * @param _current The current or starting angle (in radians).  Can lie outside the 0 to 2*PI range.
-     * @param _target The target angle (in radians) the algorithm will step towards.  Can lie outside the 0 to 2*PI range.
-     * @param _stepsize The maximum step size that can be taken (in radians).
-     * @return The new angle (in radians) for {@code _current} after performing the specified step towards the specified target.
-     * This value will always lie in the range 0 to 2*PI (exclusive).
-     */
-    public static double StepTowardsCircular(double _current, double _target, double _stepsize) {
-        _current = WrapAngle(_current);
-        _target = WrapAngle(_target);
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 
-        double stepDirection = Math.signum(_target - _current);
-        double difference = Math.abs(_current - _target);
-
-        if (difference <= _stepsize) {
-            return _target;
-        }
-        else if (difference > Math.PI) { //does the system need to wrap over eventually?
-            //handle the special case where you can reach the target in one step while also wrapping
-            if (_current + 2*Math.PI - _target < _stepsize || _target + 2*Math.PI - _current < _stepsize) {
-                return _target;
-            }
-            else {
-                return WrapAngle(_current - stepDirection * _stepsize); //this will handle wrapping gracefully
-            }
-
-        }
-        else {
-            return _current + stepDirection * _stepsize;
-        }
-    }
+public class SwerveUtils {
 
     /**
-     * Finds the (unsigned) minimum difference between two angles including calculating across 0.
-     * @param _angleA An angle (in radians).
-     * @param _angleB An angle (in radians).
-     * @return The (unsigned) minimum difference between the two angles (in radians).
+     * Limit the linear and angular accelerations of the chassis between setpoints.
+     * @param target - the target chassis velocity for the next period
+     * @param previous - the previous target chassis velocity
+     * @param dt - the time interval between periods
+     * @param linear_acc_lim - the maximum linear acceleration allowed
+     * @param rotational_acc_lim - the maximum rotational acceleration allowed
      */
-    public static double AngleDifference(double _angleA, double _angleB) {
-        double difference = Math.abs(_angleA - _angleB);
-        return difference > Math.PI? (2 * Math.PI) - difference : difference;
+    public static void RateLimitVelocity(ChassisSpeeds target, ChassisSpeeds previous, double dt, double linear_acc_lim, double rotational_acc_lim) {
+        final double
+            dvlim = linear_acc_lim * dt,        // the maximum change in velocity allowed for the given time interval
+            drlim = rotational_acc_lim * dt,
+            dvx = target.vxMetersPerSecond - previous.vxMetersPerSecond,    // the current change in velocity (components)
+            dvy = target.vyMetersPerSecond - previous.vyMetersPerSecond,
+            dvr = target.omegaRadiansPerSecond - previous.omegaRadiansPerSecond,
+            dv = Math.hypot(dvx, dvy),                  // the current change in velocity (vector magnitude)
+            _dv = MathUtil.clamp(dv, -dvlim, dvlim),    // the clamped magnitude
+            _dr = MathUtil.clamp(dvr, -drlim, drlim),
+            scale = dv == 0.0 ? 1.0 : _dv / dv,         // protect against div by 0 when delta velocity was (0, 0)
+            _dvx = dvx * scale,                         // rescale component deltas based on clamped magnitude
+            _dvy = dvy * scale;
+        target.vxMetersPerSecond = previous.vxMetersPerSecond + _dvx;       // reapply clamped changes in velocity
+        target.vyMetersPerSecond = previous.vyMetersPerSecond + _dvy;
+        target.omegaRadiansPerSecond = previous.omegaRadiansPerSecond + _dr;
     }
 
-    /**
-     * Wraps an angle until it lies within the range from 0 to 2*PI (exclusive).
-     * @param _angle The angle (in radians) to wrap.  Can be positive or negative and can lie multiple wraps outside the output range.
-     * @return An angle (in radians) from 0 and 2*PI (exclusive).
-     */
-    public static double WrapAngle(double _angle) {
-        double twoPi = 2*Math.PI;
-
-        if (_angle == twoPi) { // Handle this case separately to avoid floating point errors with the floor after the division in the case below
-            return 0.0;
-        }
-        else if (_angle > twoPi) {
-            return _angle - twoPi*Math.floor(_angle / twoPi);
-        }
-        else if (_angle < 0.0) {
-            return _angle + twoPi*(Math.floor((-_angle) / twoPi)+1);
-        }
-        else {
-            return _angle;
-        }
-    }
 }