Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add timestamp getter to MathShared (#5091)

Browse Source 
This makes it possible to mock the timestamp for wpimath without affecting the rest of the library.

Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
This commit is contained in:
Jonah
2023-02-17 17:53:17 -05:00
committed by GitHub
parent 9cc14bbb43
commit e9a7bed988
16 changed files with 76 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpimath/src/main/java/edu/wpi/first/math/MathShared.java
View File

@@ -20,4 +20,11 @@ public interface MathShared {
* @param count the usage count
*/
void reportUsage(MathUsageId id, int count);
/**
* Get the current time.
*
* @return Time in seconds
*/
double getTimestamp();
}

16
wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
View File

@@ -4,6 +4,8 @@
package edu.wpi.first.math;
import edu.wpi.first.util.WPIUtilJNI;
public final class MathSharedStore {
private static MathShared mathShared;
@@ -23,6 +25,11 @@ public final class MathSharedStore {
@Override
public void reportUsage(MathUsageId id, int count) {}
@Override
public double getTimestamp() {
return WPIUtilJNI.now() * 1.0e-6;
}
};
}
return mathShared;
@@ -56,4 +63,13 @@ public final class MathSharedStore {
public static void reportUsage(MathUsageId id, int count) {
getMathShared().reportUsage(id, count);
}
/**
* Get the time.
*
* @return The time in seconds.
*/
public static double getTimestamp() {
return getMathShared().getTimestamp();
}
}

4
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -17,7 +18,6 @@ import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
@@ -292,7 +292,7 @@ public class DifferentialDrivePoseEstimator {
public Pose2d update(
Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
return updateWithTime(
WPIUtilJNI.now() * 1.0e-6, gyroAngle, distanceLeftMeters, distanceRightMeters);
MathSharedStore.getTimestamp(), gyroAngle, distanceLeftMeters, distanceRightMeters);
}
/**

4
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -18,7 +19,6 @@ import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
@@ -269,7 +269,7 @@ public class MecanumDrivePoseEstimator {
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelPositions);
return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions);
}
/**

4
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -18,7 +19,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.Arrays;
import java.util.Map;
import java.util.NoSuchElementException;
@@ -271,7 +271,7 @@ public class SwerveDrivePoseEstimator {
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, modulePositions);
return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, modulePositions);
}
/**

6
wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.math.filter;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.math.MathSharedStore;
/**
* A simple debounce filter for boolean streams. Requires that the boolean change value from
@@ -60,11 +60,11 @@ public class Debouncer {
}
private void resetTimer() {
m_prevTimeSeconds = WPIUtilJNI.now() * 1e-6;
m_prevTimeSeconds = MathSharedStore.getTimestamp();
}
private boolean hasElapsed() {
return (WPIUtilJNI.now() * 1e-6) - m_prevTimeSeconds >= m_debounceTimeSeconds;
return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds;
}
/**

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

@@ -4,8 +4,8 @@
package edu.wpi.first.math.filter;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.WPIUtilJNI;
/**
* A class that limits the rate of change of an input value. Useful for implementing voltage,
@@ -33,7 +33,7 @@ public class SlewRateLimiter {
m_positiveRateLimit = positiveRateLimit;
m_negativeRateLimit = negativeRateLimit;
m_prevVal = initialValue;
m_prevTime = WPIUtilJNI.now() * 1e-6;
m_prevTime = MathSharedStore.getTimestamp();
}
/**
@@ -67,7 +67,7 @@ public class SlewRateLimiter {
* @return The filtered value, which will not change faster than the slew rate.
*/
public double calculate(double input) {
double currentTime = WPIUtilJNI.now() * 1e-6;
double currentTime = MathSharedStore.getTimestamp();
double elapsedTime = currentTime - m_prevTime;
m_prevVal +=
MathUtil.clamp(
@@ -85,6 +85,6 @@ public class SlewRateLimiter {
*/
public void reset(double value) {
m_prevVal = value;
m_prevTime = WPIUtilJNI.now() * 1e-6;
m_prevTime = MathSharedStore.getTimestamp();
}
}