diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java index d3aa7d8808..668b8b1ea2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java @@ -14,29 +14,50 @@ import edu.wpi.first.util.WPIUtilJNI; * edu.wpi.first.math.trajectory.TrapezoidProfile} instead. */ public class SlewRateLimiter { - private final double m_rateLimit; + private final double m_positiveRateLimit; + private final double m_negativeRateLimit; private double m_prevVal; private double m_prevTime; /** - * Creates a new SlewRateLimiter with the given rate limit and initial value. + * Creates a new SlewRateLimiter with the given positive and negative rate limits and initial + * value. * - * @param rateLimit The rate-of-change limit, in units per second. + * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per + * second. This is expected to be positive. + * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per + * second. This is expected to be negative. * @param initialValue The initial value of the input. */ - public SlewRateLimiter(double rateLimit, double initialValue) { - m_rateLimit = rateLimit; + public SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) { + m_positiveRateLimit = positiveRateLimit; + m_negativeRateLimit = negativeRateLimit; m_prevVal = initialValue; m_prevTime = WPIUtilJNI.now() * 1e-6; } /** - * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero. + * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of + * -rateLimit and initial value. + * + * @param rateLimit The rate-of-change limit, in units per second. + * @param initalValue The initial value of the input. + * @deprecated Use SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double + * initalValue) instead. + */ + @Deprecated(since = "2023", forRemoval = true) + public SlewRateLimiter(double rateLimit, double initalValue) { + this(rateLimit, -rateLimit, initalValue); + } + + /** + * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of + * -rateLimit. * * @param rateLimit The rate-of-change limit, in units per second. */ public SlewRateLimiter(double rateLimit) { - this(rateLimit, 0); + this(rateLimit, -rateLimit, 0); } /** @@ -49,7 +70,10 @@ public class SlewRateLimiter { double currentTime = WPIUtilJNI.now() * 1e-6; double elapsedTime = currentTime - m_prevTime; m_prevVal += - MathUtil.clamp(input - m_prevVal, -m_rateLimit * elapsedTime, m_rateLimit * elapsedTime); + MathUtil.clamp( + input - m_prevVal, + m_negativeRateLimit * elapsedTime, + m_positiveRateLimit * elapsedTime); m_prevTime = currentTime; return m_prevVal; } diff --git a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h index f99c1af3d2..542cd9453d 100644 --- a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h +++ b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h @@ -6,6 +6,7 @@ #include +#include #include #include "units/time.h" @@ -28,15 +29,45 @@ class SlewRateLimiter { using Rate_t = units::unit_t; /** - * Creates a new SlewRateLimiter with the given rate limit and initial value. + * Creates a new SlewRateLimiter with the given positive and negative rate + * limits and initial value. + * + * @param positiveRateLimit The rate-of-change limit in the positive + * direction, in units per second. This is expected + * to be positive. + * @param negativeRateLimit The rate-of-change limit in the negative + * direction, in units per second. This is expected + * to be negative. + * @param initialValue The initial value of the input. + */ + SlewRateLimiter(Rate_t positiveRateLimit, Rate_t negativeRateLimit, + Unit_t initialValue = Unit_t{0}) + : m_positiveRateLimit{positiveRateLimit}, + m_negativeRateLimit{negativeRateLimit}, + m_prevVal{initialValue}, + m_prevTime{units::microsecond_t(wpi::Now())} {} + + /** + * Creates a new SlewRateLimiter with the given positive rate limit and + * negative rate limit of -rateLimit. + * + * @param rateLimit The rate-of-change limit. + */ + explicit SlewRateLimiter(Rate_t rateLimit) + : SlewRateLimiter(rateLimit, -rateLimit) {} + + /** + * Creates a new SlewRateLimiter with the given positive rate limit and + * negative rate limit of -rateLimit and initial value. * * @param rateLimit The rate-of-change limit. * @param initialValue The initial value of the input. */ - explicit SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue = Unit_t{0}) - : m_rateLimit{rateLimit}, - m_prevVal{initialValue}, - m_prevTime{units::microsecond_t(wpi::Now())} {} + WPI_DEPRECATED( + "Use SlewRateLimiter(Rate_t positiveRateLimit, Rate_t negativeRateLimit, " + "Unit_t initalValue) instead") + SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue) + : SlewRateLimiter(rateLimit, -rateLimit, initialValue) {} /** * Filters the input to limit its slew rate. @@ -48,8 +79,9 @@ class SlewRateLimiter { Unit_t Calculate(Unit_t input) { units::second_t currentTime = units::microsecond_t(wpi::Now()); units::second_t elapsedTime = currentTime - m_prevTime; - m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * elapsedTime, - m_rateLimit * elapsedTime); + m_prevVal += + std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime, + m_positiveRateLimit * elapsedTime); m_prevTime = currentTime; return m_prevVal; } @@ -66,7 +98,8 @@ class SlewRateLimiter { } private: - Rate_t m_rateLimit; + Rate_t m_positiveRateLimit; + Rate_t m_negativeRateLimit; Unit_t m_prevVal; units::second_t m_prevTime; }; diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java index e7ec644ca8..6e94b3cce6 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java @@ -38,4 +38,13 @@ class SlewRateLimiterTest { WPIUtilJNI.setMockTime(1000000L); assertEquals(limiter.calculate(0.5), 0.5); } + + @Test + void slewRatePositiveNegativeTest() { + var limiter = new SlewRateLimiter(1, -0.5, 0); + WPIUtilJNI.setMockTime(1000000L); + assertEquals(limiter.calculate(2), 1); + WPIUtilJNI.setMockTime(2000000L); + assertEquals(limiter.calculate(0), 0.5); + } } diff --git a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp index 4896073ba3..5dbe8c8847 100644 --- a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp +++ b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp @@ -38,3 +38,15 @@ TEST_F(SlewRateLimiterTest, SlewRateNoLimit) { EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m); } + +TEST_F(SlewRateLimiterTest, SlewRatePositveNegativeLimit) { + frc::SlewRateLimiter limiter(1_mps, -0.5_mps); + + now += 1_s; + + EXPECT_EQ(limiter.Calculate(2_m), 1_m); + + now += 1_s; + + EXPECT_EQ(limiter.Calculate(0_m), 0.5_m); +}