Merge pull request #9 from REVrobotics/rate-limiting

Add rate limiting

CHANGELOG.md

@@ -0,0 +1,15 @@
+# MAXSwerve C++ Template Changelog
+
+## [2023.1] - 2023-02-03
+
+### Added
+
+- Added a configurable rate limiting system to prevent excessive loads from causing premature wheel failure.
+
+### Fixed
+
+- Turning SPARK MAX not using the correct feedback device.
+
+## [2023.0] - 2023-01-18
+
+Initial release of MAXSwerve robot project.

README.md

@@ -1,4 +1,8 @@
-# MAXSwerve C++ Template
+# MAXSwerve C++ Template v2023.1
+
+See [the online changelog](https://github.com/REVrobotics/MAXSwerve-Cpp-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/cpp/RobotContainer.cpp

@@ -34,13 +34,14 @@ RobotContainer::RobotContainer() {
   // Turning is controlled by the X axis of the right stick.
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.Drive(-units::meters_per_second_t{frc::ApplyDeadband(
-                          m_driverController.GetLeftY(), 0.06)},
-                      -units::meters_per_second_t{frc::ApplyDeadband(
-                          m_driverController.GetLeftX(), 0.06)},
-                      -units::radians_per_second_t{frc::ApplyDeadband(
-                          m_driverController.GetRightX(), 0.06)},
-                      true);
+        m_drive.Drive(
+            -units::meters_per_second_t{frc::ApplyDeadband(
+                m_driverController.GetLeftY(), OIConstants::kDriveDeadband)},
+            -units::meters_per_second_t{frc::ApplyDeadband(
+                m_driverController.GetLeftX(), OIConstants::kDriveDeadband)},
+            -units::radians_per_second_t{frc::ApplyDeadband(
+                m_driverController.GetRightX(), OIConstants::kDriveDeadband)},
+            true, true);
       },
       {&m_drive}));
 }
@@ -95,5 +96,6 @@ frc2::Command* RobotContainer::GetAutonomousCommand() {
   return new frc2::SequentialCommandGroup(
       std::move(swerveControllerCommand),
       frc2::InstantCommand(
-          [this]() { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {}));
+          [this]() { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false, false); },
+          {}));
 }

src/main/cpp/subsystems/DriveSubsystem.cpp

@@ -10,6 +10,7 @@
 #include <units/velocity.h>
 
 #include "Constants.h"
+#include "utils/SwerveUtils.h"
 
 using namespace DriveConstants;
 
@@ -37,18 +38,80 @@ void DriveSubsystem::Periodic() {
 
 void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
                            units::meters_per_second_t ySpeed,
-                           units::radians_per_second_t rot,
-                           bool fieldRelative) {
-  // Adjust input based on max speed
-  xSpeed *= kMaxSpeed.value();
-  ySpeed *= kMaxSpeed.value();
-  rot *= kMaxAngularSpeed.value();
+                           units::radians_per_second_t rot, bool fieldRelative,
+                           bool rateLimit) {
+  double xSpeedCommanded;
+  double ySpeedCommanded;
+
+  if (rateLimit) {
+    // Convert XY to polar for rate limiting
+    double inputTranslationDir = atan2(ySpeed.value(), xSpeed.value());
+    double inputTranslationMag =
+        sqrt(pow(xSpeed.value(), 2) + pow(ySpeed.value(), 2));
+
+    // Calculate the direction slew rate based on an estimate of the lateral
+    // acceleration
+    double directionSlewRate;
+    if (m_currentTranslationMag != 0.0) {
+      directionSlewRate =
+          abs(DriveConstants::kDirectionSlewRate / m_currentTranslationMag);
+    } else {
+      directionSlewRate = 500.0;  // some high number that means the slew rate
+                                  // is effectively instantaneous
+    }
+
+    double currentTime = wpi::Now() * 1e-6;
+    double elapsedTime = currentTime - m_prevTime;
+    double angleDif = SwerveUtils::AngleDifference(inputTranslationDir,
+                                                   m_currentTranslationDir);
+    if (angleDif < 0.45 * std::numbers::pi) {
+      m_currentTranslationDir = SwerveUtils::StepTowardsCircular(
+          m_currentTranslationDir, inputTranslationDir,
+          directionSlewRate * elapsedTime);
+      m_currentTranslationMag = m_magLimiter.Calculate(inputTranslationMag);
+    } else if (angleDif > 0.85 * std::numbers::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 + std::numbers::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 * cos(m_currentTranslationDir);
+    ySpeedCommanded = m_currentTranslationMag * sin(m_currentTranslationDir);
+    m_currentRotation = m_rotLimiter.Calculate(rot.value());
+
+  } else {
+    xSpeedCommanded = xSpeed.value();
+    ySpeedCommanded = ySpeed.value();
+    m_currentRotation = rot.value();
+  }
+
+  // Convert the commanded speeds into the correct units for the drivetrain
+  units::meters_per_second_t xSpeedDelivered =
+      xSpeedCommanded * DriveConstants::kMaxSpeed;
+  units::meters_per_second_t ySpeedDelivered =
+      ySpeedCommanded * DriveConstants::kMaxSpeed;
+  units::radians_per_second_t rotDelivered =
+      m_currentRotation * DriveConstants::kMaxAngularSpeed;
 
   auto states = kDriveKinematics.ToSwerveModuleStates(
-      fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-                          xSpeed, ySpeed, rot,
-                          frc::Rotation2d(units::radian_t{m_gyro.GetAngle()}))
-                    : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+      fieldRelative
+          ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+                xSpeedDelivered, ySpeedDelivered, rotDelivered,
+                frc::Rotation2d(units::radian_t{m_gyro.GetAngle()}))
+          : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered});
 
   kDriveKinematics.DesaturateWheelSpeeds(&states, DriveConstants::kMaxSpeed);
 

src/main/cpp/utils/SwerveUtils.cpp

@@ -0,0 +1,66 @@
+#include "utils/SwerveUtils.h"
+
+#include <cmath>
+#include <numbers>
+
+double SwerveUtils::StepTowards(double _current, double _target,
+                                double _stepsize) {
+  if (abs(_current - _target) <= _stepsize) {
+    return _target;
+  } else if (_target < _current) {
+    return _current - _stepsize;
+  } else {
+    return _current + _stepsize;
+  }
+}
+
+double SwerveUtils::StepTowardsCircular(double _current, double _target,
+                                        double _stepsize) {
+  _current = WrapAngle(_current);
+  _target = WrapAngle(_target);
+
+  double temp = _target - _current;
+  double stepDirection = temp > 0 ? 1 : temp < 0 ? -1 : 0;  // Get sign
+  double difference = abs(_current - _target);
+
+  if (difference <= _stepsize) {
+    return _target;
+  } else if (difference > std::numbers::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 * std::numbers::pi - _target < _stepsize ||
+        _target + 2 * std::numbers::pi - _current < _stepsize) {
+      return _target;
+    } else {
+      return WrapAngle(_current -
+                       stepDirection *
+                           _stepsize);  // this will handle wrapping gracefully
+    }
+
+  } else {
+    return _current + stepDirection * _stepsize;
+  }
+}
+
+double SwerveUtils::AngleDifference(double _angleA, double _angleB) {
+  double difference = abs(_angleA - _angleB);
+  return difference > std::numbers::pi ? (2 * std::numbers::pi) - difference
+                                       : difference;
+}
+
+double SwerveUtils::WrapAngle(double _angle) {
+  double twoPi = 2 * std::numbers::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 * floor(_angle / twoPi);
+  } else if (_angle < 0.0) {
+    return _angle + twoPi * (floor((-_angle) / twoPi) + 1);
+  } else {
+    return _angle;
+  }
+}

src/main/include/Constants.h

@@ -30,6 +30,10 @@ namespace DriveConstants {
 constexpr units::meters_per_second_t kMaxSpeed = 4.8_mps;
 constexpr units::radians_per_second_t kMaxAngularSpeed{2 * std::numbers::pi};
 
+constexpr double kDirectionSlewRate = 1.2;   // radians per second
+constexpr double kMagnitudeSlewRate = 1.8;   // percent per second (1 = 100%)
+constexpr double kRotationalSlewRate = 2.0;  // percent per second (1 = 100%)
+
 // Chassis configuration
 constexpr units::meter_t kTrackWidth =
     0.6731_m;  // Distance between centers of right and left wheels on robot
@@ -133,4 +137,5 @@ extern const frc::TrapezoidProfile<units::radians>::Constraints
 
 namespace OIConstants {
 constexpr int kDriverControllerPort = 0;
+constexpr double kDriveDeadband = 0.05;
 }  // namespace OIConstants

src/main/include/subsystems/DriveSubsystem.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <frc/ADIS16470_IMU.h>
+#include <frc/filter/SlewRateLimiter.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/geometry/Rotation2d.h>
 #include <frc/kinematics/ChassisSpeeds.h>
@@ -36,10 +37,11 @@ class DriveSubsystem : public frc2::SubsystemBase {
    * @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.
    */
   void Drive(units::meters_per_second_t xSpeed,
              units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
-             bool fieldRelative);
+             bool fieldRelative, bool rateLimit);
 
   /**
    * Sets the wheels into an X formation to prevent movement.
@@ -111,6 +113,17 @@ class DriveSubsystem : public frc2::SubsystemBase {
   // The gyro sensor
   frc::ADIS16470_IMU m_gyro;
 
+  // Slew rate filter variables for controlling lateral acceleration
+  double m_currentRotation = 0.0;
+  double m_currentTranslationDir = 0.0;
+  double m_currentTranslationMag = 0.0;
+
+  frc::SlewRateLimiter<units::scalar> m_magLimiter{
+      DriveConstants::kMagnitudeSlewRate / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{
+      DriveConstants::kRotationalSlewRate / 1_s};
+  double m_prevTime = wpi::Now() * 1e-6;
+
   // Odometry class for tracking robot pose
   // 4 defines the number of modules
   frc::SwerveDriveOdometry<4> m_odometry;

src/main/include/utils/SwerveUtils.h

@@ -0,0 +1,51 @@
+class SwerveUtils {
+ public:
+  /**
+   * 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.
+   */
+  static double StepTowards(double _current, double _target, double _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).
+   */
+  static double StepTowardsCircular(double _current, double _target,
+                                    double _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).
+   */
+  static double AngleDifference(double _angleA, double _angleB);
+
+  /**
+   * 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).
+   */
+  static double WrapAngle(double _angle);
+};