[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};