Add slew rate limiting (#179)

* Add slew rate limiting * Fix comment and add constants * Add check for parameter in drive method * Remove parameter comment * Fix unit error 😳 --------- Co-authored-by: 5690 <[email protected]>
SubZero-Robotics · Jul 31, 2024 · 3bd5746 · 3bd5746
1 parent ca73733
commit 3bd5746
Show file tree

Hide file tree

Showing 4 changed files with 103 additions and 5 deletions.
diff --git a/common b/common
diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp
@@ -170,8 +170,92 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
     }
   }
 
+  double xSpeedCommanded;
+  double ySpeedCommanded;
+
+  if (rateLimit) {
+    // Convers X and Y coordinates to polar coordinates; direction for vector
+    double inputTranslationDir = atan2(ySpeed.value(), xSpeed.value());
+
+    double inputTranslationMag = std::hypot(xSpeed.value(), ySpeed.value());
+
+    double slewRateDirection;
+
+    // Adapts slew rate depending on speed. If the translation magnitude (if the
+    // robot is going fast) is quite high for example, a change in intended
+    // magnitude will take longer as to not damage wheels
+    if (m_currentTranslationMagnitude != 0.0) {
+      slewRateDirection =
+          abs(kDirectionSlewRate / m_currentTranslationMagnitude);
+    } else {
+      // Some arbitarily high value so the robot can move from a sitting
+      // position quickly
+      slewRateDirection = kPlaceHolderSlewRate;
+    }
+
+    double currentTime = wpi::Now() * kMicrosecondsToSecondsCoefficient;
+    double elapsedTime = currentTime - m_prevTime;
+
+    double angleDif = SwerveUtils::AngleDifference(
+        inputTranslationDir, m_currentTranslationDirection);
+
+    if (angleDif < kSmallAngleDif) {
+      // Sets a target direction with a step size calculated to ensure that the
+      // slew rate with increase as time does so the robot's acceleration is
+      // limited by the amount of time it has been making a manuver
+      m_currentTranslationDirection = SwerveUtils::StepTowardsCircular(
+          m_currentTranslationDirection, inputTranslationDir,
+          slewRateDirection * elapsedTime);
+
+      m_currentTranslationMagnitude =
+          m_magLimiter.Calculate(inputTranslationMag);
+    } else if (angleDif > kLargeAngleDif) {
+      if (m_currentTranslationMagnitude > kSignificantMagnitudeThreshold) {
+        // Avoids floating point errors, keeps the translation direction
+        // unchanged. Movement is significant so allows slow down to be gradual
+        m_currentTranslationMagnitude = m_magLimiter.Calculate(0.0);
+      } else {
+        // Wraps 180 degrees from the current robot directions, calculates
+        // magnitude slew rate because the current translation magnitude is
+        // not significant
+        m_currentTranslationDirection = SwerveUtils::WrapAngle(
+            m_currentTranslationDirection + std::numbers::pi);
+        m_currentTranslationMagnitude =
+            m_magLimiter.Calculate(inputTranslationMag);
+      }
+    } else {
+      m_currentTranslationDirection = SwerveUtils::StepTowardsCircular(
+          m_currentTranslationDirection, inputTranslationDir,
+          slewRateDirection * elapsedTime);
+      m_currentTranslationMagnitude = m_magLimiter.Calculate(0.0);
+    }
+    m_prevTime = currentTime;
+
+    xSpeedCommanded =
+        m_currentTranslationMagnitude * cos(m_currentTranslationDirection);
+    ySpeedCommanded =
+        m_currentTranslationMagnitude * sin(m_currentTranslationDirection);
+    m_currentRotation = m_rotLimiter.Calculate(rot.value());
+
+  } else {
+    xSpeedCommanded = xSpeed.value();
+    ySpeedCommanded = ySpeed.value();
+    m_currentRotation = rot.value();
+  }
+
+  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 = m_driveKinematics.ToSwerveModuleStates(
-      frc::ChassisSpeeds::Discretize(joystickSpeeds, dif));
+      fieldRelative
+          ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+                xSpeedDelivered, ySpeedDelivered, rotDelivered,
+                frc::Rotation2d(units::degree_t{m_gyro1.GetAngle()}))
+          : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered});
 
   driveLoopTime = now;
 
@@ -184,6 +268,8 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
   m_rearLeft.SetDesiredState(bl);
   m_rearRight.SetDesiredState(br);
 
+  driveLoopTime = now;
+
   logMotorState(m_frontLeft, std::string("Front Left Motor"));
   logMotorState(m_frontRight, std::string("Front Right Motor"));
   logMotorState(m_rearLeft, std::string("Rear Left Motor"));

diff --git a/src/main/include/constants/DriveConstants.h b/src/main/include/constants/DriveConstants.h
@@ -51,4 +51,10 @@ constexpr double kLoopsPerSecond = 1_s / kLoopTime;
 const std::vector<std::string> kMotorNames = {
     "Front-Left-Drive", "Rear-Left-Drive", "Front-Right-Drive",
     "Rear-Right-Drive"};
+
+constexpr double kSmallAngleDif = 0.45 * std::numbers::pi;
+constexpr double kLargeAngleDif = 0.85 * std::numbers::pi;
+constexpr double kPlaceHolderSlewRate = 500.0;
+constexpr double kSignificantMagnitudeThreshold = 1e-4;
+constexpr double kMicrosecondsToSecondsCoefficient = 1e-6;
 }  // namespace DriveConstants
diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h
@@ -192,16 +192,22 @@ class DriveSubsystem : public frc2::SubsystemBase {
   // time last loop took, "deltatime"
   units::second_t driveLoopTime = DriveConstants::kLoopTime;
 
+  frc::SlewRateLimiter<units::scalar> m_magnitudeLimiter{
+      DriveConstants::kMagnitudeSlewRate / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotationLimiter{
+      DriveConstants::kRotationalSlewRate / 1_s};
+
   // Slew rate filter variables for controlling lateral acceleration
   double m_currentRotation = 0.0;
-  double m_currentTranslationDir = 0.0;
-  double m_currentTranslationMag = 0.0;
+  double m_currentTranslationDirection = 0.0;
+  double m_currentTranslationMagnitude = 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;
+  double m_prevTime =
+      wpi::Now() * DriveConstants::kMicrosecondsToSecondsCoefficient;
 
   // Odometry class for tracking robot pose
   // 4 defines the number of modules
+70 −0		lib/cpp/subzero/drivetrain/SwerveUtils.cpp
+14 −14		lib/include/subzero/drivetrain/SwerveUtils.h