add makeshift motor rate limiter

roboticsmgci · Apr 25, 2022 · 0888704 · 0888704
1 parent 43e3b4a
commit 0888704
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 0 deletions.
diff --git a/src/main/cpp/subsystems/Drivetrain.cpp b/src/main/cpp/subsystems/Drivetrain.cpp
@@ -4,6 +4,7 @@
 
 #include "subsystems/Drivetrain.h"
 
+#include <cmath>
 #include <frc/Joystick.h>
 #include <frc/smartdashboard/SmartDashboard.h>
 
@@ -23,7 +24,24 @@ Drivetrain::Drivetrain() {
 
 
 void Drivetrain::Drive(double left, double right) {
+
+    if (left > m_lastSpeedLeft + m_speedRateLimit){
+        left = m_lastSpeedLeft + m_speedRateLimit;
+    } else if (left < m_lastSpeedLeft - m_speedRateLimit){
+        left = m_lastSpeedLeft - m_speedRateLimit;
+    }
+
+    if (right > m_lastSpeedRight + m_speedRateLimit){
+        right = m_lastSpeedRight + m_speedRateLimit;
+    } else if (right < m_lastSpeedRight - m_speedRateLimit){
+        right = m_lastSpeedRight - m_speedRateLimit;
+    }
+
     m_robotDrive.TankDrive(-left, right);
+
+    m_lastSpeedLeft = left;
+    m_lastSpeedRight = right;
+
 }
 
 

diff --git a/src/main/include/subsystems/Drivetrain.h b/src/main/include/subsystems/Drivetrain.h
@@ -3,6 +3,7 @@
 #include <AHRS.h>
 #include <frc/controller/PIDController.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/filter/SlewRateLimiter.h>
 #include <frc/SPI.h>
 #include <frc2/command/SubsystemBase.h>
 #include <rev/CANSparkMax.h>
@@ -36,6 +37,11 @@ class Drivetrain: public frc2::SubsystemBase {
 
         frc::DifferentialDrive m_robotDrive{m_leftLeadMotor, m_rightLeadMotor};
 
+        // Makeshift Slew Rate Limiter
+        double m_lastSpeedLeft;
+        double m_lastSpeedRight;
+        const double m_speedRateLimit = 0.08;
+
     public:
 
         AHRS m_navX{frc::SPI::Port::kMXP};