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);
+};