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

[wpimath] Fix PID atSetpoint to not return true prematurely (#4906)

Browse Source 
Wait until setpoint and measurement have been set.
This commit is contained in:
sciencewhiz
2023-01-13 22:26:30 -08:00
committed by GitHub
parent e5a6197633
commit 1e05b21ab5
5 changed files with 21 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/cpp/controller/PIDController.cpp
View File

@@ -78,6 +78,7 @@ double PIDController::GetVelocityTolerance() const {
void PIDController::SetSetpoint(double setpoint) {
m_setpoint = setpoint;
m_haveSetpoint = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -95,7 +96,8 @@ double PIDController::GetSetpoint() const {
}
bool PIDController::AtSetpoint() const {
return std::abs(m_positionError) < m_positionTolerance &&
return m_haveMeasurement && m_haveSetpoint &&
std::abs(m_positionError) < m_positionTolerance &&
std::abs(m_velocityError) < m_velocityTolerance;
}
@@ -137,6 +139,7 @@ double PIDController::GetVelocityError() const {
double PIDController::Calculate(double measurement) {
m_measurement = measurement;
m_prevError = m_positionError;
m_haveMeasurement = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -167,6 +170,7 @@ void PIDController::Reset() {
m_prevError = 0;
m_totalError = 0;
m_velocityError = 0;
m_haveMeasurement = false;
}
void PIDController::InitSendable(wpi::SendableBuilder& builder) {