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

[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

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

@@ -55,6 +55,9 @@ public class PIDController implements Sendable, AutoCloseable {
private double m_setpoint;
private double m_measurement;
private boolean m_haveMeasurement;
private boolean m_haveSetpoint;
/**
* Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
* 0.02 seconds.
@@ -199,6 +202,7 @@ public class PIDController implements Sendable, AutoCloseable {
*/
public void setSetpoint(double setpoint) {
m_setpoint = setpoint;
m_haveSetpoint = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -227,7 +231,9 @@ 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
return m_haveMeasurement
&& m_haveSetpoint
&& Math.abs(m_positionError) < m_positionTolerance
&& Math.abs(m_velocityError) < m_velocityTolerance;
}
@@ -333,6 +339,7 @@ public class PIDController implements Sendable, AutoCloseable {
public double calculate(double measurement) {
m_measurement = measurement;
m_prevError = m_positionError;
m_haveMeasurement = true;
if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -360,6 +367,7 @@ public class PIDController implements Sendable, AutoCloseable {
m_prevError = 0;
m_totalError = 0;
m_velocityError = 0;
m_haveMeasurement = false;
}
@Override

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) {

3
wpimath/src/main/native/include/frc/controller/PIDController.h
View File

@@ -252,6 +252,9 @@ class WPILIB_DLLEXPORT PIDController
double m_setpoint = 0;
double m_measurement = 0;
bool m_haveSetpoint = false;
bool m_haveMeasurement = false;
};
} // namespace frc2