From c7a1dfc0bc910fd8c1b6ed9a46670ed76fba9033 Mon Sep 17 00:00:00 2001 From: Oblarg Date: Sun, 29 Dec 2019 14:36:28 -0500 Subject: [PATCH] Add SlewRateLimiter class (#2192) This is extremely useful for implementing various "ramping" functions (such as voltage ramps, setpoint ramps, etc). Usage is straightforward; it behaves like all of our other filter classes. C++ version is unit-safe. --- .../main/native/include/frc/SlewRateLimiter.h | 74 +++++++++++++++++++ .../test/native/cpp/SlewRateLimiterTest.cpp | 27 +++++++ .../wpi/first/wpilibj/SlewRateLimiter.java | 67 +++++++++++++++++ .../first/wpilibj/SlewRateLimiterTest.java | 29 ++++++++ 4 files changed, 197 insertions(+) create mode 100644 wpilibc/src/main/native/include/frc/SlewRateLimiter.h create mode 100644 wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java diff --git a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h new file mode 100644 index 0000000000..7236a30a20 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h @@ -0,0 +1,74 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include + +#include + +namespace frc { +/** + * A class that limits the rate of change of an input value. Useful for + * implementing voltage, setpoint, and/or output ramps. A slew-rate limit + * is most appropriate when the quantity being controlled is a velocity or + * a voltage; when controlling a position, consider using a TrapezoidProfile + * instead. + * + * @see TrapezoidProfile + */ +template +class SlewRateLimiter { + using Unit_t = units::unit_t; + using Rate = units::compound_unit>; + using Rate_t = units::unit_t; + + public: + /** + * Creates a new SlewRateLimiter with the given rate limit 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_timer.Start(); + } + + /** + * Filters the input to limit its slew rate. + * + * @param input The input value whose slew rate is to be limited. + * @return The filtered value, which will not change faster than the slew + * rate. + */ + Unit_t Calculate(Unit_t input) { + m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * m_timer.Get(), + m_rateLimit * m_timer.Get()); + m_timer.Reset(); + return m_prevVal; + } + + /** + * Resets the slew rate limiter to the specified value; ignores the rate limit + * when doing so. + * + * @param value The value to reset to. + */ + void Reset(Unit_t value) { + m_timer.Reset(); + m_prevVal = value; + } + + private: + frc2::Timer m_timer; + Rate_t m_rateLimit; + Unit_t m_prevVal; +}; +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp new file mode 100644 index 0000000000..ae253a7445 --- /dev/null +++ b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/SlewRateLimiter.h" +#include "gtest/gtest.h" + +TEST(SlewRateLimiterTest, SlewRateLimitTest) { + frc::SlewRateLimiter limiter(1_mps); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + EXPECT_TRUE(limiter.Calculate(2_m) < 2_m); +} + +TEST(SlewRateLimiterTest, SlewRateNoLimitTest) { + frc::SlewRateLimiter limiter(1_mps); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java new file mode 100644 index 0000000000..2ba468b35b --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.wpiutil.math.MathUtil; + +/** + * A class that limits the rate of change of an input value. Useful for implementing voltage, + * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being + * controlled is a velocity or a voltage; when controlling a position, consider using a + * {@link edu.wpi.first.wpilibj.trajectory.TrapezoidProfile} instead. + */ +public class SlewRateLimiter { + private final Timer m_timer = new Timer(); + private final double m_rateLimit; + private double m_prevVal; + + /** + * Creates a new SlewRateLimiter with the given rate limit and initial value. + * + * @param rateLimit The rate-of-change limit, in units per second. + * @param initialValue The initial value of the input. + */ + public SlewRateLimiter(double rateLimit, double initialValue) { + m_prevVal = initialValue; + m_rateLimit = rateLimit; + m_timer.start(); + } + + /** + * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero. + * + * @param rateLimit The rate-of-change limit, in units per second. + */ + public SlewRateLimiter(double rateLimit) { + this(rateLimit, 0); + } + + /** + * Filters the input to limit its slew rate. + * + * @param input The input value whose slew rate is to be limited. + * @return The filtered value, which will not change faster than the slew rate. + */ + public double calculate(double input) { + m_prevVal += MathUtil.clamp(input - m_prevVal, + -m_rateLimit * m_timer.get(), + m_rateLimit * m_timer.get()); + m_timer.reset(); + return m_prevVal; + } + + /** + * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so. + * + * @param value The value to reset to. + */ + public void reset(double value) { + m_timer.reset(); + m_prevVal = value; + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java new file mode 100644 index 0000000000..e47b70b91c --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +public class SlewRateLimiterTest { + @Test + void slewRateLimitTest() { + SlewRateLimiter limiter = new SlewRateLimiter(1); + Timer.delay(1); + assertTrue(limiter.calculate(2) < 2); + } + + @Test + void slewRateNoLimitTest() { + SlewRateLimiter limiter = new SlewRateLimiter(1); + Timer.delay(1); + assertEquals(limiter.calculate(0.5), 0.5); + } +}