From 2f81f2b78a63ff8cb535ed56ca0d08b9212174e9 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 2 Aug 2020 00:06:52 -0700
Subject: [PATCH] [wpilib] Fix timestamp slew in SlewRateLimiter (#2627)

Fixes #2626.
---
 .../main/native/include/frc/SlewRateLimiter.h  | 18 ++++++++++--------
 .../edu/wpi/first/wpilibj/SlewRateLimiter.java | 18 ++++++++++--------
 2 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
index fe3a458973..d18fb09fd3 100644
--- a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
+++ b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
@@ -37,9 +37,9 @@ class SlewRateLimiter {
    * @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();
-  }
+      : m_rateLimit{rateLimit},
+        m_prevVal{initialValue},
+        m_prevTime{frc2::Timer::GetFPGATimestamp()} {}
 
   /**
    * Filters the input to limit its slew rate.
@@ -49,9 +49,11 @@ class SlewRateLimiter {
    * 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();
+    units::second_t currentTime = frc2::Timer::GetFPGATimestamp();
+    units::second_t elapsedTime = currentTime - m_prevTime;
+    m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * elapsedTime,
+                            m_rateLimit * elapsedTime);
+    m_prevTime = currentTime;
     return m_prevVal;
   }
 
@@ -62,13 +64,13 @@ class SlewRateLimiter {
    * @param value The value to reset to.
    */
   void Reset(Unit_t value) {
-    m_timer.Reset();
     m_prevVal = value;
+    m_prevTime = frc2::Timer::GetFPGATimestamp();
   }
 
  private:
-  frc2::Timer m_timer;
   Rate_t m_rateLimit;
   Unit_t m_prevVal;
+  units::second_t m_prevTime;
 };
 }  // namespace frc
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
index 2ba468b35b..fe4dda7400 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -16,9 +16,9 @@ import edu.wpi.first.wpiutil.math.MathUtil;
  * {@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;
+  private double m_prevTime;
 
   /**
    * Creates a new SlewRateLimiter with the given rate limit and initial value.
@@ -27,9 +27,9 @@ public class SlewRateLimiter {
    * @param initialValue The initial value of the input.
    */
   public SlewRateLimiter(double rateLimit, double initialValue) {
-    m_prevVal = initialValue;
     m_rateLimit = rateLimit;
-    m_timer.start();
+    m_prevVal = initialValue;
+    m_prevTime = Timer.getFPGATimestamp();
   }
 
   /**
@@ -48,10 +48,12 @@ public class SlewRateLimiter {
    * @return The filtered value, which will not change faster than the slew rate.
    */
   public double calculate(double input) {
+    double currentTime = Timer.getFPGATimestamp();
+    double elapsedTime = currentTime - m_prevTime;
     m_prevVal += MathUtil.clamp(input - m_prevVal,
-                                -m_rateLimit * m_timer.get(),
-                                m_rateLimit * m_timer.get());
-    m_timer.reset();
+                                -m_rateLimit * elapsedTime,
+                                m_rateLimit * elapsedTime);
+    m_prevTime = currentTime;
     return m_prevVal;
   }
 
@@ -61,7 +63,7 @@ public class SlewRateLimiter {
    * @param value The value to reset to.
    */
   public void reset(double value) {
-    m_timer.reset();
     m_prevVal = value;
+    m_prevTime = Timer.getFPGATimestamp();
   }
 }