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

Wait until setpoint and measurement have been set.

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

@@ -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

wpimath/src/main/native/cpp/controller/PIDController.cpp

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

wpimath/src/main/native/include/frc/controller/PIDController.h

@@ -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

wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java

@@ -19,7 +19,7 @@ class PIDToleranceTest {
     try (var controller = new PIDController(0.05, 0.0, 0.0)) {
       controller.enableContinuousInput(-kRange / 2, kRange / 2);
 
-      assertTrue(controller.atSetpoint());
+      assertFalse(controller.atSetpoint());
     }
   }
 
@@ -28,10 +28,7 @@ class PIDToleranceTest {
     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);

wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp

@@ -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);