Clean up PIDController interface in preparation for ProfiledPIDController

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

@@ -18,11 +18,7 @@ using namespace frc2;
 
 PIDController::PIDController(double Kp, double Ki, double Kd,
                              units::second_t period)
-    : frc::SendableBase(false),
-      m_Kp(Kp),
-      m_Ki(Ki),
-      m_Kd(Kd),
-      m_period(period.to<double>()) {
+    : frc::SendableBase(false), m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
   static int instances = 0;
   instances++;
   HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
@@ -45,8 +41,6 @@ units::second_t PIDController::GetPeriod() const {
   return units::second_t(m_period);
 }
 
-double PIDController::GetOutput() const { return m_output; }
-
 void PIDController::SetSetpoint(double setpoint) {
   if (m_maximumInput > m_minimumInput) {
     m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
@@ -57,25 +51,20 @@ void PIDController::SetSetpoint(double setpoint) {
 
 double PIDController::GetSetpoint() const { return m_setpoint; }
 
-bool PIDController::AtSetpoint(double tolerance, double deltaTolerance,
+bool PIDController::AtSetpoint(double positionTolerance,
+                               double velocityTolerance,
                                Tolerance toleranceType) const {
-  double deltaError = GetDeltaError();
-
-  if (toleranceType == Tolerance::kPercent) {
-    return std::abs(m_currError) < tolerance / 100 * m_inputRange &&
-           std::abs(deltaError) < deltaTolerance / 100 * m_inputRange;
+  if (m_toleranceType == Tolerance::kPercent) {
+    return std::abs(m_positionError) < positionTolerance / 100 * m_inputRange &&
+           std::abs(m_velocityError) < velocityTolerance / 100 * m_inputRange;
   } else {
-    return std::abs(m_currError) < tolerance &&
-           std::abs(deltaError) < deltaTolerance;
+    return std::abs(m_positionError) < positionTolerance &&
+           std::abs(m_velocityError) < velocityTolerance;
   }
 }
 
 bool PIDController::AtSetpoint() const {
-  return AtSetpoint(m_tolerance, m_deltaTolerance, m_toleranceType);
-}
-
-void PIDController::SetContinuous(bool continuous) {
-  m_continuous = continuous;
+  return AtSetpoint(m_positionTolerance, m_velocityTolerance);
 }
 
 void PIDController::SetInputRange(double minimumInput, double maximumInput) {
@@ -89,36 +78,53 @@ void PIDController::SetInputRange(double minimumInput, double maximumInput) {
   }
 }
 
+void PIDController::EnableContinuousInput(double minimumInput,
+                                          double maximumInput) {
+  m_continuous = true;
+  SetInputRange(minimumInput, maximumInput);
+}
+
+void PIDController::DisableContinuousInput() { m_continuous = false; }
+
 void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) {
   m_minimumOutput = minimumOutput;
   m_maximumOutput = maximumOutput;
 }
 
-void PIDController::SetAbsoluteTolerance(double tolerance,
-                                         double deltaTolerance) {
+void PIDController::SetAbsoluteTolerance(double positionTolerance,
+                                         double velocityTolerance) {
   m_toleranceType = Tolerance::kAbsolute;
-  m_tolerance = tolerance;
-  m_deltaTolerance = deltaTolerance;
+  m_positionTolerance = positionTolerance;
+  m_velocityTolerance = velocityTolerance;
 }
 
-void PIDController::SetPercentTolerance(double tolerance,
-                                        double deltaTolerance) {
+void PIDController::SetPercentTolerance(double positionTolerance,
+                                        double velocityTolerance) {
   m_toleranceType = Tolerance::kPercent;
-  m_tolerance = tolerance;
-  m_deltaTolerance = deltaTolerance;
+  m_positionTolerance = positionTolerance;
+  m_velocityTolerance = velocityTolerance;
 }
 
-double PIDController::GetError() const {
-  return GetContinuousError(m_currError);
+double PIDController::GetPositionError() const {
+  return GetContinuousError(m_positionError);
 }
 
-/**
- * Returns the change in error per second of the PIDController.
- *
- * @return The change in error per second.
- */
-double PIDController::GetDeltaError() const {
-  return (m_currError - m_prevError) / m_period;
+double PIDController::GetVelocityError() const { return m_velocityError; }
+
+double PIDController::Calculate(double measurement) {
+  m_prevError = m_positionError;
+  m_positionError = GetContinuousError(m_setpoint - measurement);
+  m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
+
+  if (m_Ki != 0) {
+    m_totalError =
+        std::clamp(m_totalError + m_positionError * m_period.to<double>(),
+                   m_minimumOutput / m_Ki, m_maximumOutput / m_Ki);
+  }
+
+  return std::clamp(
+      m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError,
+      m_minimumOutput, m_maximumOutput);
 }
 
 double PIDController::Calculate(double measurement, double setpoint) {
@@ -130,7 +136,6 @@ double PIDController::Calculate(double measurement, double setpoint) {
 void PIDController::Reset() {
   m_prevError = 0;
   m_totalError = 0;
-  m_output = 0;
 }
 
 void PIDController::InitSendable(frc::SendableBuilder& builder) {
@@ -159,19 +164,3 @@ double PIDController::GetContinuousError(double error) const {
 
   return error;
 }
-
-double PIDController::Calculate(double measurement) {
-  m_prevError = m_currError;
-  m_currError = GetContinuousError(m_setpoint - measurement);
-
-  if (m_Ki != 0) {
-    m_totalError = std::clamp(m_totalError + m_currError * m_period,
-                              m_minimumOutput / m_Ki, m_maximumOutput / m_Ki);
-  }
-
-  m_output = std::clamp(m_Kp * m_currError + m_Ki * m_totalError +
-                            m_Kd * (m_currError - m_prevError) / m_period,
-                        m_minimumOutput, m_maximumOutput);
-
-  return m_output;
-}

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

@@ -30,7 +30,7 @@ class PIDController : public frc::SendableBase {
    * @param Ki     The integral coefficient.
    * @param Kd     The derivative coefficient.
    * @param period The period between controller updates in seconds. The
-   *               default is 0.02 seconds.
+   *               default is 20 milliseconds.
    */
   PIDController(double Kp, double Ki, double Kd,
                 units::second_t period = 20_ms);
@@ -102,16 +102,6 @@ class PIDController : public frc::SendableBase {
    */
   units::second_t GetPeriod() const;
 
-  /**
-   * Returns the current controller output.
-   *
-   * This is always centered around zero and constrained to the min and max
-   * outputs.
-   *
-   * @return The latest calculated output.
-   */
-  double GetOutput() const;
-
   /**
    * Sets the setpoint for the PIDController.
    *
@@ -131,37 +121,22 @@ class PIDController : public frc::SendableBase {
    *
    * This will return false until at least one input value has been computed.
    *
-   * @param tolerance The maximum allowable error.
-   * @param deltaTolerance The maximum allowable change in error.
-   * @param toleranceType The type of tolerances specified.
+   * @param positionTolerance The maximum allowable position error.
+   * @param velocityTolerance The maximum allowable velocity error.
+   * @param toleranceType     The type of tolerance specified.
    */
   bool AtSetpoint(
-      double tolerance,
-      double deltaTolerance = std::numeric_limits<double>::infinity(),
+      double positionTolerance,
+      double velocityTolerance = std::numeric_limits<double>::infinity(),
       Tolerance toleranceType = Tolerance::kAbsolute) const;
 
   /**
    * Returns true if the error is within the tolerance of the error.
    *
-   * Currently this just reports on target as the actual value passes through
-   * the setpoint. Ideally it should be based on being within the tolerance for
-   * some period of time.
-   *
    * This will return false until at least one input value has been computed.
    */
   bool AtSetpoint() const;
 
-  /**
-   * Sets the PID controller to consider the input to be continuous.
-   *
-   * Rather then using the max and min input range as constraints, it considers
-   * them to be the same point and automatically calculates the shortest route
-   * to the setpoint.
-   *
-   * @param continuous true turns on continuous, false turns off continuous
-   */
-  void SetContinuous(bool continuous = true);
-
   /**
    * Sets the minimum and maximum values expected from the input.
    *
@@ -170,6 +145,23 @@ class PIDController : public frc::SendableBase {
    */
   void SetInputRange(double minimumInput, double maximumInput);
 
+  /**
+   * Enables continuous input.
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  void EnableContinuousInput(double minimumInput, double maximumInput);
+
+  /**
+   * Disables continuous input.
+   */
+  void DisableContinuousInput();
+
   /**
    * Sets the minimum and maximum values to write.
    *
@@ -182,36 +174,33 @@ class PIDController : public frc::SendableBase {
    * Sets the absolute error which is considered tolerable for use with
    * AtSetpoint().
    *
-   * @param tolerance      Error which is tolerable.
-   * @param deltaTolerance Change in error per second which is tolerable.
+   * @param positionTolerance Position error which is tolerable.
+   * @param velociytTolerance Velocity error which is tolerable.
    */
   void SetAbsoluteTolerance(
-      double tolerance,
-      double deltaTolerance = std::numeric_limits<double>::infinity());
+      double positionTolerance,
+      double velocityTolerance = std::numeric_limits<double>::infinity());
 
   /**
-   * Sets the percentage error which is considered tolerable for use with
+   * Sets the percent error which is considered tolerable for use with
    * AtSetpoint().
    *
-   * @param tolerance      Percent error which is tolerable.
-   * @param deltaTolerance Change in percent error per second which is
-   *                       tolerable.
+   * @param positionTolerance Position error which is tolerable.
+   * @param velociytTolerance Velocity error which is tolerable.
    */
   void SetPercentTolerance(
-      double tolerance,
-      double deltaTolerance = std::numeric_limits<double>::infinity());
+      double positionTolerance,
+      double velocityTolerance = std::numeric_limits<double>::infinity());
 
   /**
    * Returns the difference between the setpoint and the measurement.
-   *
-   * @return The error.
    */
-  double GetError() const;
+  double GetPositionError() const;
 
   /**
-   * Returns the change in error per second.
+   * Returns the velocity error.
    */
-  double GetDeltaError() const;
+  double GetVelocityError() const;
 
   /**
    * Returns the next output of the PID controller.
@@ -256,7 +245,7 @@ class PIDController : public frc::SendableBase {
   double m_Kd;
 
   // The period (in seconds) of the control loop running this controller
-  double m_period;
+  units::second_t m_period;
 
   // |maximum output|
   double m_maximumOutput = 1.0;
@@ -270,30 +259,30 @@ class PIDController : public frc::SendableBase {
   // Minimum input - limit setpoint to this
   double m_minimumInput = 0;
 
-  // input range - difference between maximum and minimum
+  // Input range - difference between maximum and minimum
   double m_inputRange = 0;
 
   // Do the endpoints wrap around? eg. Absolute encoder
   bool m_continuous = false;
 
-  // The error at the time of the most recent call to calculate()
-  double m_currError = 0;
+  // The error at the time of the most recent call to Calculate()
+  double m_positionError = 0;
+  double m_velocityError = 0;
 
-  // The error at the time of the second-most-recent call to calculate() (used
+  // The error at the time of the second-most-recent call to Calculate() (used
   // to compute velocity)
-  double m_prevError = std::numeric_limits<double>::infinity();
+  double m_prevError = 0;
 
   // The sum of the errors for use in the integral calc
   double m_totalError = 0;
 
   Tolerance m_toleranceType = Tolerance::kAbsolute;
 
-  // The percentage or absolute error that is considered at setpoint.
-  double m_tolerance = 0.05;
-  double m_deltaTolerance = std::numeric_limits<double>::infinity();
+  // The error that is considered at setpoint.
+  double m_positionTolerance = 0.05;
+  double m_velocityTolerance = std::numeric_limits<double>::infinity();
 
   double m_setpoint = 0;
-  double m_output = 0;
 };
 
 }  // namespace frc2

wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp

@@ -36,8 +36,7 @@ TEST_F(PIDInputOutputTest, InputRangeTest) {
 
 TEST_F(PIDInputOutputTest, ContinuousInputTest) {
   controller->SetP(1);
-  controller->SetInputRange(-180, 180);
-  controller->SetContinuous(true);
+  controller->EnableContinuousInput(-180, 180);
 
   EXPECT_TRUE(controller->Calculate(-179, 179) < 0);
 }
@@ -65,6 +64,6 @@ TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) {
 
   controller->Calculate(0, 0);
 
-  EXPECT_DOUBLE_EQ(-.01 / controller->GetPeriod().to<double>(),
+  EXPECT_DOUBLE_EQ(-.01_s / controller->GetPeriod(),
                    controller->Calculate(.0025, 0));
 }

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

@@ -5,105 +5,71 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include "frc/Timer.h"
 #include "frc/controller/PIDController.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
 class PIDToleranceTest : public testing::Test {
  protected:
-  const double setpoint = 50.0;
-  const double range = 200;
-  const double tolerance = 10.0;
+  static constexpr double kSetpoint = 50.0;
+  static constexpr double kRange = 200;
+  static constexpr double kTolerance = 10.0;
 
-  class FakeInput {
-   public:
-    double val = 0;
-
-    double Get() { return val; }
-  };
-
-  class FakeOutput {
-   public:
-    void Set(double output) {}
-  };
-
-  FakeInput inp;
-  FakeOutput out;
   frc2::PIDController* pidController;
 
   void SetUp() override {
     pidController = new frc2::PIDController(0.5, 0.0, 0.0);
-    pidController->SetInputRange(-range / 2, range / 2);
+    pidController->SetInputRange(-kRange / 2, kRange / 2);
   }
 
   void TearDown() override { delete pidController; }
-
-  void Reset() { inp.val = 0; }
 };
 
-TEST_F(PIDToleranceTest, Initial) { EXPECT_FALSE(pidController->AtSetpoint()); }
+TEST_F(PIDToleranceTest, Initial) { EXPECT_TRUE(pidController->AtSetpoint()); }
 
 TEST_F(PIDToleranceTest, Absolute) {
-  Reset();
+  pidController->SetAbsoluteTolerance(kTolerance);
+  pidController->SetSetpoint(kSetpoint);
 
-  pidController->SetAbsoluteTolerance(tolerance);
-  pidController->SetSetpoint(setpoint);
+  pidController->Calculate(0.0);
 
   EXPECT_FALSE(pidController->AtSetpoint())
       << "Error was in tolerance when it should not have been. Error was "
-      << pidController->GetError();
+      << pidController->GetPositionError();
 
-  inp.val = setpoint + tolerance / 2;
-  for (int i = 0; i <= 50; i++) {
-    pidController->Calculate(inp.Get());
-  }
+  pidController->Calculate(kSetpoint + kTolerance / 2);
 
   EXPECT_TRUE(pidController->AtSetpoint())
       << "Error was not in tolerance when it should have been. Error was "
-      << pidController->GetError();
+      << pidController->GetPositionError();
 
-  inp.val = setpoint + 10 * tolerance;
-  for (int i = 0; i <= 50; i++) {
-    pidController->Calculate(inp.Get());
-  }
+  pidController->Calculate(kSetpoint + 10 * kTolerance);
 
   EXPECT_FALSE(pidController->AtSetpoint())
       << "Error was in tolerance when it should not have been. Error was "
-      << pidController->GetError();
+      << pidController->GetPositionError();
 }
 
 TEST_F(PIDToleranceTest, Percent) {
-  Reset();
+  pidController->SetPercentTolerance(kTolerance);
+  pidController->SetSetpoint(kSetpoint);
 
-  pidController->SetPercentTolerance(tolerance);
-  pidController->SetSetpoint(setpoint);
+  pidController->Calculate(0.0);
 
   EXPECT_FALSE(pidController->AtSetpoint())
       << "Error was in tolerance when it should not have been. Error was "
-      << pidController->GetError();
+      << pidController->GetPositionError();
 
-  inp.val =
-      setpoint + (tolerance) / 200 *
-                     range;  // half of percent tolerance away from setpoint
-  for (int i = 0; i <= 50; i++) {
-    pidController->Calculate(inp.Get());
-  }
+  // Half of percent tolerance away from setpoint
+  pidController->Calculate(kSetpoint + (kTolerance / 2) / 100 * kRange);
 
   EXPECT_TRUE(pidController->AtSetpoint())
       << "Error was not in tolerance when it should have been. Error was "
-      << pidController->GetError();
+      << pidController->GetPositionError();
 
-  inp.val =
-      setpoint +
-      (tolerance) / 50 * range;  // double percent tolerance away from setPoint
-
-  for (int i = 0; i <= 50; i++) {
-    pidController->Calculate(inp.Get());
-  }
+  // Double percent tolerance away from setpoint
+  pidController->Calculate(kSetpoint + (kTolerance * 2) / 100 * kRange);
 
   EXPECT_FALSE(pidController->AtSetpoint())
       << "Error was in tolerance when it should not have been. Error was "
-      << pidController->GetError();
+      << pidController->GetPositionError();
 }