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

[wpilib] Fix timestamp slew in SlewRateLimiter (#2627)

Browse Source 
Fixes #2626.
This commit is contained in:
Tyler Veness
2020-08-02 00:06:52 -07:00
committed by GitHub
parent aba035eb3d
commit 2f81f2b78a
2 changed files with 20 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
View File

@@ -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();
}
}