Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11: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
wpilibc/src/main/native/include/frc/SlewRateLimiter.h
View File

@@ -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

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