[wpilib] Fix ProfiledPIDController continuous input (#2652)

There were three bugs:

1. The input range variables used in ProfiledPIDController::Calculate()
   weren't being updated
2. The modulus error calculation was incorrect.
3. The setpoint wasn't being wrapped like the goal, so the invariant
   that the error remains less than half the input range was violated.
   (Thanks to @CptJJ for pointing this out and suggesting a fix.)

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

@@ -18,12 +18,6 @@ namespace frc {
  * Returns modulus of error where error is the difference between the reference
  * and a measurement.
  *
- * This implements modular subtraction defined as:
- *
- * e = (r mod m - x mod m) mod m
- *
- * with an offset in the modulus range for minimum input.
- *
  * @param reference Reference input of a controller.
  * @param measurement The current measurement.
  * @param minimumInput The minimum value expected from the input.
@@ -31,28 +25,18 @@ namespace frc {
  */
 template <typename T>
 T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) {
+  T error = reference - measurement;
   T modulus = maximumInput - minimumInput;
 
-  if constexpr (std::is_same_v<T, double>) {
-    T error = std::fmod(reference, modulus) - std::fmod(measurement, modulus);
+  // Wrap error above maximum input
+  int numMax = (error + maximumInput) / modulus;
+  error -= numMax * modulus;
 
-    // Moduli on the difference arguments establish a precondition for the
-    // following modulus.
-    return std::fmod(error - minimumInput, modulus) + minimumInput;
-  } else if constexpr (std::is_same_v<T, int>) {
-    T error = reference % modulus - measurement % modulus;
+  // Wrap error below minimum input
+  int numMin = (error + minimumInput) / modulus;
+  error -= numMin * modulus;
 
-    // Moduli on the difference arguments establish a precondition for the
-    // following modulus.
-    return (error - minimumInput) % modulus + minimumInput;
-  } else {
-    T error = units::math::fmod(reference, modulus) -
-              units::math::fmod(measurement, modulus);
-
-    // Moduli on the difference arguments establish a precondition for the
-    // following modulus.
-    return units::math::fmod(error - minimumInput, modulus) + minimumInput;
-  }
+  return error;
 }
 
 }  // namespace frc

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

@@ -196,6 +196,8 @@ class ProfiledPIDController
   void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
     m_controller.EnableContinuousInput(minimumInput.template to<double>(),
                                        maximumInput.template to<double>());
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
   }
 
   /**
@@ -254,8 +256,10 @@ class ProfiledPIDController
   double Calculate(Distance_t measurement) {
     if (m_controller.IsContinuousInputEnabled()) {
       // Get error which is smallest distance between goal and measurement
-      auto error = frc::GetModulusError<Distance_t>(
+      auto goalMinDistance = frc::GetModulusError<Distance_t>(
           m_goal.position, measurement, m_minimumInput, m_maximumInput);
+      auto setpointMinDistance = frc::GetModulusError<Distance_t>(
+          m_setpoint.position, measurement, m_minimumInput, m_maximumInput);
 
       // Recompute the profile goal with the smallest error, thus giving the
       // shortest path. The goal may be outside the input range after this
@@ -263,7 +267,8 @@ class ProfiledPIDController
       // report an error of zero. In other words, the setpoint only needs to be
       // offset from the measurement by the input range modulus; they don't need
       // to be equal.
-      m_goal.position = Distance_t{error} + measurement;
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
     }
 
     frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};

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

@@ -17,6 +17,11 @@ TEST(ControllerUtilTest, GetModulusError) {
                   frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
   EXPECT_FLOAT_EQ(-20.0,
                   frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0, frc::GetModulusError(-170.0, 170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0,
+                  frc::GetModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0,
+                  frc::GetModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0));
 
   // Test range starting at zero
   EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0));

wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.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.                                                               */
@@ -21,7 +21,7 @@ TEST_F(PIDInputOutputTest, ContinuousInputTest) {
   controller->SetP(1);
   controller->EnableContinuousInput(-180, 180);
 
-  EXPECT_TRUE(controller->Calculate(-179, 179) < 0);
+  EXPECT_LT(controller->Calculate(-179, 179), 0);
 }
 
 TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {

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

@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <units/angle.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <wpi/math>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "gtest/gtest.h"
+
+class ProfiledPIDInputOutputTest : public testing::Test {
+ protected:
+  frc::ProfiledPIDController<units::degrees>* controller;
+
+  void SetUp() override {
+    controller = new frc::ProfiledPIDController<units::degrees>(
+        0, 0, 0, {360_deg_per_s, 180_deg_per_s_sq});
+  }
+
+  void TearDown() override { delete controller; }
+};
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180_deg, 180_deg);
+
+  controller->Reset(-179_deg);
+  EXPECT_LT(controller->Calculate(-179_deg, 179_deg), 0);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest1) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180_deg, 180_deg);
+
+  static constexpr units::degree_t kSetpoint{-179.0};
+  static constexpr units::degree_t kMeasurement{-179.0};
+  static constexpr units::degree_t kGoal{179.0};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            180_deg);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest2) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::math::pi},
+                                    units::radian_t{wpi::math::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.4826633343199735};
+  static constexpr units::radian_t kMeasurement{-3.1352207333939606};
+  static constexpr units::radian_t kGoal{-3.534162788601621};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::math::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest3) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::math::pi},
+                                    units::radian_t{wpi::math::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.5176604690006377};
+  static constexpr units::radian_t kMeasurement{3.1191729343822456};
+  static constexpr units::radian_t kGoal{2.709680418117445};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::math::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutputTest) {
+  controller->SetP(4);
+
+  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025_deg, 0_deg));
+}
+
+TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutputTest) {
+  controller->SetI(4);
+
+  double out = 0;
+
+  for (int i = 0; i < 5; i++) {
+    out = controller->Calculate(0.025_deg, 0_deg);
+  }
+
+  EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutputTest) {
+  controller->SetD(4);
+
+  controller->Calculate(0_deg, 0_deg);
+
+  EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+                   controller->Calculate(0.0025_deg, 0_deg));
+}

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

@@ -12,12 +12,6 @@ public final class ControllerUtil {
    * Returns modulus of error where error is the difference between the reference
    * and a measurement.
    *
-   * <p>This implements modular subtraction defined as:
-   *
-   * <p>e = (r mod m - x mod m) mod m
-   *
-   * <p>with an offset in the modulus range for minimum input.
-   *
    * @param reference Reference input of a controller.
    * @param measurement The current measurement.
    * @param minimumInput The minimum value expected from the input.
@@ -25,12 +19,18 @@ public final class ControllerUtil {
    */
   public static double getModulusError(double reference, double measurement, double minimumInput,
       double maximumInput) {
+    double error = reference - measurement;
     double modulus = maximumInput - minimumInput;
-    double error = reference % modulus - measurement % modulus;
 
-    // Moduli on the difference arguments establish a precondition for the
-    // following modulus.
-    return (error - minimumInput) % modulus + minimumInput;
+    // Wrap error above maximum input
+    int numMax = (int) ((error + maximumInput) / modulus);
+    error -= numMax * modulus;
+
+    // Wrap error below minimum input
+    int numMin = (int) ((error + minimumInput) / modulus);
+    error -= numMin * modulus;
+
+    return error;
   }
 
   private ControllerUtil() {

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

@@ -218,6 +218,8 @@ public class ProfiledPIDController implements Sendable {
    */
   public void enableContinuousInput(double minimumInput, double maximumInput) {
     m_controller.enableContinuousInput(minimumInput, maximumInput);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
   }
 
   /**
@@ -283,14 +285,17 @@ public class ProfiledPIDController implements Sendable {
   public double calculate(double measurement) {
     if (m_controller.isContinuousInputEnabled()) {
       // Get error which is smallest distance between goal and measurement
-      double error = ControllerUtil.getModulusError(m_goal.position, measurement, m_minimumInput,
-          m_maximumInput);
+      double goalMinDistance = ControllerUtil.getModulusError(m_goal.position, measurement,
+          m_minimumInput, m_maximumInput);
+      double setpointMinDistance = ControllerUtil.getModulusError(m_setpoint.position, measurement,
+          m_minimumInput, m_maximumInput);
 
       // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal
       // may be outside the input range after this operation, but that's OK because the controller
       // will still go there and report an error of zero. In other words, the setpoint only needs to
       // be offset from the measurement by the input range modulus; they don't need to be equal.
-      m_goal.position = error + measurement;
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
     }
 
     var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);

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

@@ -18,6 +18,9 @@ class ControllerUtilTest {
     assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -180.0, 180.0));
     assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
     assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
+    assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0, -180.0, 180.0));
+    assertEquals(20.0, ControllerUtil.getModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0));
+    assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0));
 
     // Test range start at zero
     assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0, 0.0, 360.0));

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

@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class ProfiledPIDInputOutputTest {
+  private ProfiledPIDController m_controller;
+
+  @BeforeEach
+  void setUp() {
+    m_controller = new ProfiledPIDController(0, 0, 0,
+        new TrapezoidProfile.Constraints(360, 180));
+  }
+
+  @Test
+  void continuousInputTest1() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-180, 180);
+
+    final double kSetpoint = -179.0;
+    final double kMeasurement = -179.0;
+    final double kGoal = 179.0;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < 180.0);
+  }
+
+  @Test
+  void continuousInputTest2() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.4826633343199735;
+    final double kMeasurement = -3.1352207333939606;
+    final double kGoal = -3.534162788601621;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void continuousInputTest3() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.5176604690006377;
+    final double kMeasurement = 3.1191729343822456;
+    final double kGoal = 2.709680418117445;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    m_controller.setP(4);
+
+    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    m_controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = m_controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    m_controller.setD(4);
+
+    m_controller.calculate(0, 0);
+
+    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
+  }
+}