Fix ProfiledPIDController profile direction for continuous input (#2279)

Previously, it could take the long way around. This recomputes the
profile goal with the shortest error, thus taking the shortest path.

Also removed the setpoint clamping from PIDController::SetSetpoint()
because it's unnecessary to make PIDController behave correctly for
a modular arithmetic input, and it breaks the setpoint calculation in
ProfiledPIDController otherwise.

Fixes #2277.

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

@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+#include <type_traits>
+
+#include <units/units.h>
+
+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.
+ * @param maximumInput The maximum value expected from the input.
+ */
+template <typename T>
+T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) {
+  T modulus = maximumInput - minimumInput;
+
+  if constexpr (std::is_same_v<T, double>) {
+    T error = std::fmod(reference, modulus) - std::fmod(measurement, 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;
+
+    // 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;
+  }
+}
+
+}  // namespace frc

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

@@ -140,6 +140,11 @@ class PIDController : public frc::Sendable,
    */
   void DisableContinuousInput();
 
+  /**
+   * Returns true if continuous input is enabled.
+   */
+  bool IsContinuousInputEnabled() const;
+
   /**
    * Sets the minimum and maximum values for the integrator.
    *
@@ -193,16 +198,6 @@ class PIDController : public frc::Sendable,
 
   void InitSendable(frc::SendableBuilder& builder) override;
 
- protected:
-  /**
-   * Wraps error around for continuous inputs. The original error is returned if
-   * continuous mode is disabled.
-   *
-   * @param error The current error of the PID controller.
-   * @return Error for continuous inputs.
-   */
-  double GetContinuousError(double error) const;
-
  private:
   // Factor for "proportional" control
   double m_Kp;
@@ -220,15 +215,10 @@ class PIDController : public frc::Sendable,
 
   double m_minimumIntegral = -1.0;
 
-  // Maximum input - limit setpoint to this
   double m_maximumInput = 0;
 
-  // Minimum input - limit setpoint to this
   double m_minimumInput = 0;
 
-  // Input range - difference between maximum and minimum
-  double m_inputRange = 0;
-
   // Do the endpoints wrap around? eg. Absolute encoder
   bool m_continuous = false;
 
@@ -248,14 +238,6 @@ class PIDController : public frc::Sendable,
   double m_velocityTolerance = std::numeric_limits<double>::infinity();
 
   double m_setpoint = 0;
-
-  /**
-   * Sets the minimum and maximum values expected from the input.
-   *
-   * @param minimumInput The minimum value expected from the input.
-   * @param maximumInput The maximum value expected from the input.
-   */
-  void SetInputRange(double minimumInput, double maximumInput);
 };
 
 }  // namespace frc2

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

@@ -14,6 +14,7 @@
 
 #include <units/units.h>
 
+#include "frc/controller/ControllerUtil.h"
 #include "frc/controller/PIDController.h"
 #include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -251,6 +252,20 @@ class ProfiledPIDController
    * @param measurement The current measurement of the process variable.
    */
   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>(
+          m_goal.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 = Distance_t{error} + measurement;
+    }
+
     frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
     m_setpoint = profile.Calculate(GetPeriod());
     return m_controller.Calculate(measurement.template to<double>(),
@@ -337,6 +352,8 @@ class ProfiledPIDController
 
  private:
   frc2::PIDController m_controller;
+  Distance_t m_minimumInput{0};
+  Distance_t m_maximumInput{0};
   typename frc::TrapezoidProfile<Distance>::State m_goal;
   typename frc::TrapezoidProfile<Distance>::State m_setpoint;
   typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;