Merge pull request #8 from REVrobotics/rate_limiting

Rate limiting

Author: jfabellera

CHANGELOG.md

@@ -0,0 +1,11 @@
+# MAXSwerve Java Template Changelog
+
+## [2023.1] - 2023-02-03
+
+### Added
+
+- Added a configurable rate limiting system to prevent excessive loads from causing premature wheel failure.
+
+## [2023.0] - 2023-01-18
+
+Initial release of MAXSwerve robot project.

README.md

@@ -1,4 +1,8 @@
-# MAXSwerve Java Template
+# MAXSwerve Java Template v2023.1
+
+See [the online changelog](https://github.com/REVrobotics/MAXSwerve-Java-Template/blob/main/CHANGELOG.md) for information about updates to the template that may have been released since you created your project.
+
+## Description
 
 A template project for an FRC swerve drivetrain that uses REV MAXSwerve Modules.
 

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

@@ -30,6 +30,10 @@ 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%)
+
     // Chassis configuration
     public static final double kTrackWidth = Units.inchesToMeters(26.5);
     // Distance between centers of right and left wheels on robot
@@ -114,6 +118,7 @@ public static final class ModuleConstants {
 
   public static final class OIConstants {
     public static final int kDriverControllerPort = 0;
+    public static final double kDriveDeadband = 0.05;
   }
 
   public static final class AutoConstants {

src/main/java/frc/robot/RobotContainer.java

@@ -51,10 +51,10 @@ public RobotContainer() {
         // Turning is controlled by the X axis of the right stick.
         new RunCommand(
             () -> m_robotDrive.drive(
-                MathUtil.applyDeadband(-m_driverController.getLeftY(), 0.06),
-                MathUtil.applyDeadband(-m_driverController.getLeftX(), 0.06),
-                MathUtil.applyDeadband(-m_driverController.getRightX(), 0.06),
-                true),
+                -MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband),
+                -MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband),
+                -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband),
+                true, true),
             m_robotDrive));
   }
 
@@ -117,6 +117,6 @@ public Command getAutonomousCommand() {
     m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
 
     // Run path following command, then stop at the end.
-    return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+    return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false, false));
   }
 }

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

@@ -4,15 +4,18 @@
 
 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;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.wpilibj.ADIS16470_IMU;
 import frc.robot.Constants.DriveConstants;
+import frc.utils.SwerveUtils;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
@@ -40,6 +43,15 @@ 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;
+
   // Odometry class for tracking robot pose
   SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
       DriveConstants.kDriveKinematics,
@@ -102,17 +114,70 @@ public void resetOdometry(Pose2d pose) {
    * @param rot           Angular rate of the robot.
    * @param fieldRelative Whether the provided x and y speeds are relative to the
    *                      field.
+   * @param rateLimit     Whether to enable rate limiting for smoother control.
    */
-  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
-    // Adjust input based on max speed
-    xSpeed *= DriveConstants.kMaxSpeedMetersPerSecond;
-    ySpeed *= DriveConstants.kMaxSpeedMetersPerSecond;
-    rot *= DriveConstants.kMaxAngularSpeed;
+  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
+    double xSpeedDelivered = xSpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond;
+    double ySpeedDelivered = ySpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond;
+    double rotDelivered = m_currentRotation * DriveConstants.kMaxAngularSpeed;
 
     var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
         fieldRelative
-            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, Rotation2d.fromDegrees(m_gyro.getAngle()))
-            : new ChassisSpeeds(xSpeed, ySpeed, rot));
+            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(m_gyro.getAngle()))
+            : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
     SwerveDriveKinematics.desaturateWheelSpeeds(
         swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
     m_frontLeft.setDesiredState(swerveModuleStates[0]);

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

@@ -0,0 +1,89 @@
+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);
+
+        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;
+        }
+    }
+
+    /**
+     * 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).
+     */
+    public static double AngleDifference(double _angleA, double _angleB) {
+        double difference = Math.abs(_angleA - _angleB);
+        return difference > Math.PI? (2 * Math.PI) - difference : difference;
+    }
+
+    /**
+     * 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;
+        }
+    }
+}