Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] PIDController: Recompute the error in AtSetpoint() (#2822)

Browse Source 
This makes AtSetpoint() return false after the setpoint is changed with
SetSetpoint().

Closes #2821.

Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
This commit is contained in:
Tyler Veness
2020-11-28 14:33:17 -08:00
committed by GitHub
parent 2056f0ce09
commit 3413bfc06a
5 changed files with 45 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
View File

@@ -62,6 +62,7 @@ public class PIDController implements Sendable, AutoCloseable {
private double m_velocityTolerance = Double.POSITIVE_INFINITY;
private double m_setpoint;
private double m_measurement;
/**
* Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of
@@ -211,8 +212,18 @@ public class PIDController implements Sendable, AutoCloseable {
* @return Whether the error is within the acceptable bounds.
*/
public boolean atSetpoint() {
return Math.abs(m_positionError) < m_positionTolerance
&& Math.abs(m_velocityError) < m_velocityTolerance;
double positionError;
if (m_continuous) {
positionError = ControllerUtil.getModulusError(m_setpoint, m_measurement, m_minimumInput,
m_maximumInput);
} else {
positionError = m_setpoint - m_measurement;
}
double velocityError = (positionError - m_prevError) / m_period;
return Math.abs(positionError) < m_positionTolerance
&& Math.abs(velocityError) < m_velocityTolerance;
}
/**
@@ -313,6 +324,7 @@ public class PIDController implements Sendable, AutoCloseable {
* @param measurement The current measurement of the process variable.
*/
public double calculate(double measurement) {
m_measurement = measurement;
m_prevError = m_positionError;
if (m_continuous) {

8
wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
View File

@@ -31,9 +31,17 @@ class PIDToleranceTest {
try (var controller = new PIDController(0.05, 0.0, 0.0)) {
controller.enableContinuousInput(-kRange / 2, kRange / 2);
assertTrue(controller.atSetpoint(),
"Error was not in tolerance when it should have been. Error was "
+ controller.getPositionError());
controller.setTolerance(kTolerance);
controller.setSetpoint(kSetpoint);
assertFalse(controller.atSetpoint(),
"Error was in tolerance when it should not have been. Error was "
+ controller.getPositionError());
controller.calculate(0.0);
assertFalse(controller.atSetpoint(),