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.

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 <frc2/Timer.h>
+
+#include <algorithm>
+
+#include <units/units.h>
+
+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 Unit>
+class SlewRateLimiter {
+  using Unit_t = units::unit_t<Unit>;
+  using Rate = units::compound_unit<Unit, units::inverse<units::seconds>>;
+  using Rate_t = units::unit_t<Rate>;
+
+ 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

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 <thread>
+
+#include "frc/SlewRateLimiter.h"
+#include "gtest/gtest.h"
+
+TEST(SlewRateLimiterTest, SlewRateLimitTest) {
+  frc::SlewRateLimiter<units::meters> 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<units::meters> limiter(1_mps);
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+  EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
+}

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;
+  }
+}

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);
+  }
+}