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

[wpimath] Fix PIDController having incorrect error after calling SetSetpoint() (#4070)

Browse Source
This commit is contained in:
Tyler Veness
2022-03-19 23:59:00 -07:00
committed by GitHub
parent cdafc723fb
commit 78108c2aba
2 changed files with 27 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
View File

@@ -181,6 +181,15 @@ public class PIDController implements Sendable, AutoCloseable {
*/
public void setSetpoint(double setpoint) {
m_setpoint = setpoint;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
m_positionError = m_setpoint - m_measurement;
}
m_velocityError = (m_positionError - m_prevError) / m_period;
}
/**
@@ -200,18 +209,8 @@ public class PIDController implements Sendable, AutoCloseable {
* @return Whether the error is within the acceptable bounds.
*/
public boolean atSetpoint() {
double positionError;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
positionError = m_setpoint - m_measurement;
}
double velocityError = (positionError - m_prevError) / m_period;
return Math.abs(positionError) < m_positionTolerance
&& Math.abs(velocityError) < m_velocityTolerance;
return Math.abs(m_positionError) < m_positionTolerance
&& Math.abs(m_velocityError) < m_velocityTolerance;
}
/**
@@ -303,8 +302,7 @@ public class PIDController implements Sendable, AutoCloseable {
* @return The next controller output.
*/
public double calculate(double measurement, double setpoint) {
// Set setpoint to provided value
setSetpoint(setpoint);
m_setpoint = setpoint;
return calculate(measurement);
}
@@ -322,7 +320,7 @@ public class PIDController implements Sendable, AutoCloseable {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
m_positionError = m_setpoint - measurement;
m_positionError = m_setpoint - m_measurement;
}
m_velocityError = (m_positionError - m_prevError) / m_period;