Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 4 commits into from
Jan 14, 2023
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
57fa451
Fix PID atSetpoint to not return true prematurely
sciencewhiz Jan 6, 2023
1f94532
Add test and fix reset
sciencewhiz Jan 6, 2023
522b994
Fix test
sciencewhiz Jan 7, 2023
199990e
C++ implementation
sciencewhiz Jan 14, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 9 additions & 1 deletion wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -199,6 +202,7 @@ public double getVelocityTolerance() {
*/
public void setSetpoint(double setpoint) {
m_setpoint = setpoint;
m_haveSetpoint = true;

if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
Expand Down Expand Up @@ -227,7 +231,9 @@ public double getSetpoint() {
* @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;
}

Expand Down Expand Up @@ -333,6 +339,7 @@ public double calculate(double measurement, double setpoint) {
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;
Expand Down Expand Up @@ -360,6 +367,7 @@ public void reset() {
m_prevError = 0;
m_totalError = 0;
m_velocityError = 0;
m_haveMeasurement = false;
}

@Override
Expand Down
6 changes: 5 additions & 1 deletion wpimath/src/main/native/cpp/controller/PIDController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand Down
3 changes: 3 additions & 0 deletions wpimath/src/main/native/include/frc/controller/PIDController.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 2 additions & 5 deletions wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ void initialToleranceTest() {
try (var controller = new PIDController(0.05, 0.0, 0.0)) {
controller.enableContinuousInput(-kRange / 2, kRange / 2);

assertTrue(controller.atSetpoint());
assertFalse(controller.atSetpoint());
}
}

Expand All @@ -28,10 +28,7 @@ void absoluteToleranceTest() {
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());
assertFalse(controller.atSetpoint());

controller.setTolerance(kTolerance);
controller.setSetpoint(kSetpoint);
Expand Down
6 changes: 2 additions & 4 deletions wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,16 +13,14 @@ TEST(PIDToleranceTest, InitialTolerance) {
frc2::PIDController controller{0.5, 0.0, 0.0};
controller.EnableContinuousInput(-kRange / 2, kRange / 2);

EXPECT_TRUE(controller.AtSetpoint());
EXPECT_FALSE(controller.AtSetpoint());
}

TEST(PIDToleranceTest, AbsoluteTolerance) {
frc2::PIDController controller{0.5, 0.0, 0.0};
controller.EnableContinuousInput(-kRange / 2, kRange / 2);

EXPECT_TRUE(controller.AtSetpoint())
<< "Error was not in tolerance when it should have been. Error was "
<< controller.GetPositionError();
EXPECT_FALSE(controller.AtSetpoint());

controller.SetTolerance(kTolerance);
controller.SetSetpoint(kSetpoint);
Expand Down