diff --git a/src/main/cpp/subsystems/Drivetrain.cpp b/src/main/cpp/subsystems/Drivetrain.cpp index 9c1bd41..dcd5933 100644 --- a/src/main/cpp/subsystems/Drivetrain.cpp +++ b/src/main/cpp/subsystems/Drivetrain.cpp @@ -4,6 +4,7 @@ #include "subsystems/Drivetrain.h" +#include #include #include @@ -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 index 95a5fc0..a1a14cf 100644 --- a/src/main/include/subsystems/Drivetrain.h +++ b/src/main/include/subsystems/Drivetrain.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -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};