Revert "Add slew rate limiting (#179)" (#181)

This reverts commit 3bd5746.
SubZero-Robotics · Nov 11, 2024 · 626499b · 626499b
1 parent 3bd5746
commit 626499b
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 103 deletions.
diff --git a/common b/common
diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp
@@ -170,92 +170,8 @@ 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(
-      fieldRelative
-          ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-                xSpeedDelivered, ySpeedDelivered, rotDelivered,
-                frc::Rotation2d(units::degree_t{m_gyro1.GetAngle()}))
-          : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered});
+      frc::ChassisSpeeds::Discretize(joystickSpeeds, dif));
 
   driveLoopTime = now;
 
@@ -268,8 +184,6 @@ 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,10 +51,4 @@ 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,22 +192,16 @@ 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_currentTranslationDirection = 0.0;
-  double m_currentTranslationMagnitude = 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() * DriveConstants::kMicrosecondsToSecondsCoefficient;
+  double m_prevTime = wpi::Now() * 1e-6;
 
   // Odometry class for tracking robot pose
   // 4 defines the number of modules