[wpilib] PIDController: Recompute the error in AtSetpoint() (#2822)

This makes AtSetpoint() return false after the setpoint is changed with
SetSetpoint().

Closes #2821.

Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>

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

@@ -54,8 +54,18 @@ void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
 double PIDController::GetSetpoint() const { return m_setpoint; }
 
 bool PIDController::AtSetpoint() const {
-  return std::abs(m_positionError) < m_positionTolerance &&
-         std::abs(m_velocityError) < m_velocityTolerance;
+  double positionError;
+  if (m_continuous) {
+    positionError = frc::GetModulusError<double>(
+        m_setpoint, m_measurement, m_minimumInput, m_maximumInput);
+  } else {
+    positionError = m_setpoint - m_measurement;
+  }
+
+  double velocityError = (positionError - m_prevError) / m_period.to<double>();
+
+  return std::abs(positionError) < m_positionTolerance &&
+         std::abs(velocityError) < m_velocityTolerance;
 }
 
 void PIDController::EnableContinuousInput(double minimumInput,
@@ -86,6 +96,7 @@ double PIDController::GetPositionError() const { return m_positionError; }
 double PIDController::GetVelocityError() const { return m_velocityError; }
 
 double PIDController::Calculate(double measurement) {
+  m_measurement = measurement;
   m_prevError = m_positionError;
 
   if (m_continuous) {

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

@@ -238,6 +238,7 @@ class PIDController : public frc::Sendable,
   double m_velocityTolerance = std::numeric_limits<double>::infinity();
 
   double m_setpoint = 0;
+  double m_measurement = 0;
 };
 
 }  // namespace frc2

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,9 +23,17 @@ 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();
+
   controller.SetTolerance(kTolerance);
   controller.SetSetpoint(kSetpoint);
 
+  EXPECT_FALSE(controller.AtSetpoint())
+      << "Error was in tolerance when it should not have been. Error was "
+      << controller.GetPositionError();
+
   controller.Calculate(0.0);
 
   EXPECT_FALSE(controller.AtSetpoint())

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java

@@ -62,6 +62,7 @@ public class PIDController implements Sendable, AutoCloseable {
   private double m_velocityTolerance = Double.POSITIVE_INFINITY;
 
   private double m_setpoint;
+  private double m_measurement;
 
   /**
    * Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of
@@ -211,8 +212,18 @@ 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
-        && Math.abs(m_velocityError) < m_velocityTolerance;
+    double positionError;
+    if (m_continuous) {
+      positionError = ControllerUtil.getModulusError(m_setpoint, m_measurement, m_minimumInput,
+          m_maximumInput);
+    } else {
+      positionError = m_setpoint - m_measurement;
+    }
+
+    double velocityError = (positionError - m_prevError) / m_period;
+
+    return Math.abs(positionError) < m_positionTolerance
+        && Math.abs(velocityError) < m_velocityTolerance;
   }
 
   /**
@@ -313,6 +324,7 @@ public class PIDController implements Sendable, AutoCloseable {
    * @param measurement The current measurement of the process variable.
    */
   public double calculate(double measurement) {
+    m_measurement = measurement;
     m_prevError = m_positionError;
 
     if (m_continuous) {

wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java

@@ -31,9 +31,17 @@ 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());
+
       controller.setTolerance(kTolerance);
       controller.setSetpoint(kSetpoint);
 
+      assertFalse(controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+          + controller.getPositionError());
+
       controller.calculate(0.0);
 
       assertFalse(controller.atSetpoint(),