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

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

@@ -70,6 +70,16 @@ units::second_t PIDController::GetPeriod() const {
void PIDController::SetSetpoint(double setpoint) {
m_setpoint = setpoint;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_positionError =
frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
m_positionError = m_setpoint - m_measurement;
}
m_velocityError = (m_positionError - m_prevError) / m_period.value();
}
double PIDController::GetSetpoint() const {
@@ -77,19 +87,8 @@ double PIDController::GetSetpoint() const {
}
bool PIDController::AtSetpoint() const {
double positionError;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
positionError =
frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
positionError = m_setpoint - m_measurement;
}
double velocityError = (positionError - m_prevError) / m_period.value();
return std::abs(positionError) < m_positionTolerance &&
std::abs(velocityError) < m_velocityTolerance;
return std::abs(m_positionError) < m_positionTolerance &&
std::abs(m_velocityError) < m_velocityTolerance;
}
void PIDController::EnableContinuousInput(double minimumInput,
@@ -136,7 +135,7 @@ double PIDController::Calculate(double measurement) {
m_positionError =
frc::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.value();
@@ -151,8 +150,7 @@ double PIDController::Calculate(double measurement) {
}
double PIDController::Calculate(double measurement, double setpoint) {
// Set setpoint to provided value
SetSetpoint(setpoint);
m_setpoint = setpoint;
return Calculate(measurement);
}