diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle index aa31984810..76359eb10a 100644 --- a/shared/jni/setupBuild.gradle +++ b/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') diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java index eed2e590d9..d573130344 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java +++ b/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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java index 2e700aeb90..cf1f729305 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java +++ b/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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java index e219a118c2..675150f22f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java +++ b/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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java index 1e758a479e..6a79162fc8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java +++ b/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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java index 24db270833..cb9e0d9f93 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java +++ b/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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java index f256cc8aff..3eed9d1dc8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java +++ b/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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java index 7bed5cff05..310ceda45f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java +++ b/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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java index 08acbaff60..c8b6622f0b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java +++ b/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; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java similarity index 90% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java rename to wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java index 849a869588..54ab44166d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java +++ b/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; } } diff --git a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h b/wpimath/src/main/native/include/frc/SlewRateLimiter.h similarity index 89% rename from wpilibc/src/main/native/include/frc/SlewRateLimiter.h rename to wpimath/src/main/native/include/frc/SlewRateLimiter.h index d1da9016c2..f99c1af3d2 100644 --- a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h +++ b/wpimath/src/main/native/include/frc/SlewRateLimiter.h @@ -6,9 +6,9 @@ #include -#include +#include -#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: diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java similarity index 60% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java rename to wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java index 73b70bcbd3..18bf7016da 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java +++ b/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); } } diff --git a/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/SlewRateLimiterTest.cpp similarity index 59% rename from wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp rename to wpimath/src/test/native/cpp/SlewRateLimiterTest.cpp index cf4a93eb4f..23525de41e 100644 --- a/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp +++ b/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 - -#include -#include -#include +#include #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(); }); + frc::SlewRateLimiter 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(); }); + frc::SlewRateLimiter limiter(1_mps); - frc::sim::StepTiming(1.0_s); + now += 1_s; EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m); } diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java index fa554ea605..b717601ab8 100644 --- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java +++ b/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); diff --git a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp b/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp index 67e91f09bf..bc7913ef94 100644 --- a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp +++ b/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(); + } } /* diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java b/wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java index 8a8c36c372..cb1f919cbf 100644 --- a/wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java +++ b/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);