[wpimath] Move SlewRateLimiter from wpilib to wpimath (#3399)

Timer was replaced with wpi::Now() to avoid a dependency on other wpilib
classes.

shared/jni/setupBuild.gradle

@@ -197,6 +197,7 @@ model {
                 lib library: "${nativeName}JNIShared", linkage: 'shared'
                 if (!project.hasProperty('noWpiutil')) {
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                    lib project: ':wpiutil', library: 'wpiutilJNIShared', linkage: 'shared'
                 }
                 if (nativeName == 'hal' && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
                     nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java

@@ -4,8 +4,8 @@
 
 package edu.wpi.first.wpilibj.examples.differentialdrivebot;
 
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java

@@ -4,8 +4,8 @@
 
 package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
 
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java

@@ -4,8 +4,8 @@
 
 package edu.wpi.first.wpilibj.examples.mecanumbot;
 
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java

@@ -4,8 +4,8 @@
 
 package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
 
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj.examples.ramsetecontroller;
 
 import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -13,7 +14,6 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
 
 import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -12,7 +13,6 @@ import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java

@@ -4,8 +4,8 @@
 
 package edu.wpi.first.wpilibj.examples.swervebot;
 
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java

@@ -4,8 +4,8 @@
 
 package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
 
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 

wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java

@@ -2,9 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package edu.wpi.first.wpilibj;
+package edu.wpi.first.math.filter;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpiutil.WPIUtilJNI;
 
 /**
  * A class that limits the rate of change of an input value. Useful for implementing voltage,
@@ -26,7 +27,7 @@ public class SlewRateLimiter {
   public SlewRateLimiter(double rateLimit, double initialValue) {
     m_rateLimit = rateLimit;
     m_prevVal = initialValue;
-    m_prevTime = Timer.getFPGATimestamp();
+    m_prevTime = WPIUtilJNI.now() * 1e-6;
   }
 
   /**
@@ -45,7 +46,7 @@ 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 currentTime = WPIUtilJNI.now() * 1e-6;
     double elapsedTime = currentTime - m_prevTime;
     m_prevVal +=
         MathUtil.clamp(input - m_prevVal, -m_rateLimit * elapsedTime, m_rateLimit * elapsedTime);
@@ -60,6 +61,6 @@ public class SlewRateLimiter {
    */
   public void reset(double value) {
     m_prevVal = value;
-    m_prevTime = Timer.getFPGATimestamp();
+    m_prevTime = WPIUtilJNI.now() * 1e-6;
   }
 }

wpimath/src/main/native/include/frc/SlewRateLimiter.h

@@ -6,9 +6,9 @@
 
 #include <algorithm>
 
-#include <units/time.h>
+#include <wpi/timestamp.h>
 
-#include "frc/Timer.h"
+#include "units/time.h"
 
 namespace frc {
 /**
@@ -36,7 +36,7 @@ class SlewRateLimiter {
   explicit SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue = Unit_t{0})
       : m_rateLimit{rateLimit},
         m_prevVal{initialValue},
-        m_prevTime{Timer::GetFPGATimestamp()} {}
+        m_prevTime{units::microsecond_t(wpi::Now())} {}
 
   /**
    * Filters the input to limit its slew rate.
@@ -46,7 +46,7 @@ class SlewRateLimiter {
    * rate.
    */
   Unit_t Calculate(Unit_t input) {
-    units::second_t currentTime = Timer::GetFPGATimestamp();
+    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);
@@ -62,7 +62,7 @@ class SlewRateLimiter {
    */
   void Reset(Unit_t value) {
     m_prevVal = value;
-    m_prevTime = Timer::GetFPGATimestamp();
+    m_prevTime = units::microsecond_t(wpi::Now());
   }
 
  private:

wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java

@@ -2,25 +2,34 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package edu.wpi.first.wpilibj;
+package edu.wpi.first.math.filter;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.wpiutil.WPIUtilJNI;
 import org.junit.jupiter.api.Test;
 
 public class SlewRateLimiterTest {
   @Test
   void slewRateLimitTest() {
-    SlewRateLimiter limiter = new SlewRateLimiter(1);
-    Timer.delay(1);
+    WPIUtilJNI.enableMockTime();
+
+    var limiter = new SlewRateLimiter(1);
+    WPIUtilJNI.setMockTime(1000000L);
     assertTrue(limiter.calculate(2) < 2);
+
+    WPIUtilJNI.setMockTime(0L);
   }
 
   @Test
   void slewRateNoLimitTest() {
-    SlewRateLimiter limiter = new SlewRateLimiter(1);
-    Timer.delay(1);
+    WPIUtilJNI.enableMockTime();
+
+    var limiter = new SlewRateLimiter(1);
+    WPIUtilJNI.setMockTime(1000000L);
     assertEquals(limiter.calculate(0.5), 0.5);
+
+    WPIUtilJNI.setMockTime(0L);
   }
 }

wpimath/src/test/native/cpp/SlewRateLimiterTest.cpp

@@ -2,28 +2,32 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include <thread>
-
-#include <units/length.h>
-#include <units/time.h>
-#include <units/velocity.h>
+#include <wpi/timestamp.h>
 
 #include "frc/SlewRateLimiter.h"
-#include "frc/simulation/SimHooks.h"
 #include "gtest/gtest.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+static units::second_t now = 0_s;
 
 TEST(SlewRateLimiterTest, SlewRateLimitTest) {
+  WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+
   frc::SlewRateLimiter<units::meters> limiter(1_mps);
 
-  frc::sim::StepTiming(1.0_s);
+  now += 1_s;
 
-  EXPECT_TRUE(limiter.Calculate(2_m) < 2_m);
+  EXPECT_LT(limiter.Calculate(2_m), 2_m);
 }
 
 TEST(SlewRateLimiterTest, SlewRateNoLimitTest) {
+  WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+
   frc::SlewRateLimiter<units::meters> limiter(1_mps);
 
-  frc::sim::StepTiming(1.0_s);
+  now += 1_s;
 
   EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
 }

wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java

@@ -50,6 +50,10 @@ public final class WPIUtilJNI {
     libraryLoaded = true;
   }
 
+  public static native void enableMockTime();
+
+  public static native void setMockTime(long time);
+
   public static native long now();
 
   public static native void addPortForwarder(int port, String remoteHost, int remotePort);

wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp

@@ -11,6 +11,9 @@
 
 using namespace wpi::java;
 
+static bool mockTimeEnabled = false;
+static uint64_t mockNow = 0;
+
 extern "C" {
 
 JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
@@ -24,6 +27,31 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
 
 JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {}
 
+/*
+ * Class:     edu_wpi_first_wpiutil_WPIUtilJNI
+ * Method:    enableMockTime
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_wpiutil_WPIUtilJNI_enableMockTime
+  (JNIEnv*, jclass)
+{
+  mockTimeEnabled = true;
+  wpi::SetNowImpl([] { return mockNow; });
+}
+
+/*
+ * Class:     edu_wpi_first_wpiutil_WPIUtilJNI
+ * Method:    setMockTime
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_wpiutil_WPIUtilJNI_setMockTime
+  (JNIEnv*, jclass, jlong time)
+{
+  mockNow = time;
+}
+
 /*
  * Class:     edu_wpi_first_wpiutil_WPIUtilJNI
  * Method:    now
@@ -33,7 +61,11 @@ JNIEXPORT jlong JNICALL
 Java_edu_wpi_first_wpiutil_WPIUtilJNI_now
   (JNIEnv*, jclass)
 {
-  return wpi::Now();
+  if (mockTimeEnabled) {
+    return mockNow;
+  } else {
+    return wpi::Now();
+  }
 }
 
 /*

wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java

@@ -9,6 +9,16 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import org.junit.jupiter.api.Test;
 
 public class WPIUtilJNITest {
+  @Test
+  public void testEnableMockTime() {
+    assertDoesNotThrow(WPIUtilJNI::enableMockTime);
+  }
+
+  @Test
+  public void testSetMockTime() {
+    assertDoesNotThrow(() -> WPIUtilJNI.setMockTime(0L));
+  }
+
   @Test
   public void testNow() {
     assertDoesNotThrow(WPIUtilJNI::now);