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/cpp/controller/PIDController.cpp

@@ -12,6 +12,7 @@
 
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/controller/ControllerUtil.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
@@ -48,13 +49,7 @@ units::second_t PIDController::GetPeriod() const {
   return units::second_t(m_period);
 }
 
-void PIDController::SetSetpoint(double setpoint) {
-  if (m_maximumInput > m_minimumInput) {
-    m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
-  } else {
-    m_setpoint = setpoint;
-  }
-}
+void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
 
 double PIDController::GetSetpoint() const { return m_setpoint; }
 
@@ -66,11 +61,14 @@ bool PIDController::AtSetpoint() const {
 void PIDController::EnableContinuousInput(double minimumInput,
                                           double maximumInput) {
   m_continuous = true;
-  SetInputRange(minimumInput, maximumInput);
+  m_minimumInput = minimumInput;
+  m_maximumInput = maximumInput;
 }
 
 void PIDController::DisableContinuousInput() { m_continuous = false; }
 
+bool PIDController::IsContinuousInputEnabled() const { return m_continuous; }
+
 void PIDController::SetIntegratorRange(double minimumIntegral,
                                        double maximumIntegral) {
   m_minimumIntegral = minimumIntegral;
@@ -83,15 +81,20 @@ void PIDController::SetTolerance(double positionTolerance,
   m_velocityTolerance = velocityTolerance;
 }
 
-double PIDController::GetPositionError() const {
-  return GetContinuousError(m_positionError);
-}
+double PIDController::GetPositionError() const { return m_positionError; }
 
 double PIDController::GetVelocityError() const { return m_velocityError; }
 
 double PIDController::Calculate(double measurement) {
   m_prevError = m_positionError;
-  m_positionError = GetContinuousError(m_setpoint - measurement);
+
+  if (m_continuous) {
+    m_positionError = frc::GetModulusError<double>(
+        m_setpoint, measurement, m_minimumInput, m_maximumInput);
+  } else {
+    m_positionError = m_setpoint - measurement;
+  }
+
   m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
 
   if (m_Ki != 0) {
@@ -125,29 +128,3 @@ void PIDController::InitSendable(frc::SendableBuilder& builder) {
   builder.AddDoubleProperty("setpoint", [this] { return GetSetpoint(); },
                             [this](double value) { SetSetpoint(value); });
 }
-
-double PIDController::GetContinuousError(double error) const {
-  if (m_continuous && m_inputRange > 0) {
-    error = std::fmod(error, m_inputRange);
-    if (std::abs(error) > m_inputRange / 2) {
-      if (error > 0) {
-        return error - m_inputRange;
-      } else {
-        return error + m_inputRange;
-      }
-    }
-  }
-
-  return error;
-}
-
-void PIDController::SetInputRange(double minimumInput, double maximumInput) {
-  m_minimumInput = minimumInput;
-  m_maximumInput = maximumInput;
-  m_inputRange = maximumInput - minimumInput;
-
-  // Clamp setpoint to new input range
-  if (m_maximumInput > m_minimumInput) {
-    m_setpoint = std::clamp(m_setpoint, m_minimumInput, m_maximumInput);
-  }
-}

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;

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

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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/units.h>
+
+#include "frc/controller/ControllerUtil.h"
+#include "gtest/gtest.h"
+
+TEST(ControllerUtilTest, GetModulusError) {
+  // Test symmetric range
+  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));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0 + 360.0, 190.0, 0.0, 360.0));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0, 190.0 + 360.0, 0.0, 360.0));
+
+  // Test asymmetric range that doesn't start at zero
+  EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -170.0, 190.0));
+
+  // Test all supported types
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError<double>(170.0, -170.0, -170.0, 190.0));
+  EXPECT_EQ(-20, frc::GetModulusError<int>(170, -170, -170, 190));
+  EXPECT_EQ(-20_deg, frc::GetModulusError<units::degree_t>(170_deg, -170_deg,
+                                                           -170_deg, 190_deg));
+}

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

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+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.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public static double getModulusError(double reference, double measurement, double minimumInput,
+      double maximumInput) {
+    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;
+  }
+
+  private ControllerUtil() {
+  }
+}

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

@@ -40,15 +40,10 @@ public class PIDController implements Sendable, AutoCloseable {
 
   private double m_minimumIntegral = -1.0;
 
-  // Maximum input - limit setpoint to this
   private double m_maximumInput;
 
-  // Minimum input - limit setpoint to this
   private double m_minimumInput;
 
-  // Input range - difference between maximum and minimum
-  private double m_inputRange;
-
   // Do the endpoints wrap around? eg. Absolute encoder
   private boolean m_continuous;
 
@@ -196,11 +191,7 @@ public class PIDController implements Sendable, AutoCloseable {
    * @param setpoint The desired setpoint.
    */
   public void setSetpoint(double setpoint) {
-    if (m_maximumInput > m_minimumInput) {
-      m_setpoint = MathUtil.clamp(setpoint, m_minimumInput, m_maximumInput);
-    } else {
-      m_setpoint = setpoint;
-    }
+    m_setpoint = setpoint;
   }
 
   /**
@@ -236,7 +227,8 @@ public class PIDController implements Sendable, AutoCloseable {
    */
   public void enableContinuousInput(double minimumInput, double maximumInput) {
     m_continuous = true;
-    setInputRange(minimumInput, maximumInput);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
   }
 
   /**
@@ -246,6 +238,13 @@ public class PIDController implements Sendable, AutoCloseable {
     m_continuous = false;
   }
 
+  /**
+   * Returns true if continuous input is enabled.
+   */
+  public boolean isContinuousInputEnabled() {
+    return m_continuous;
+  }
+
   /**
    * Sets the minimum and maximum values for the integrator.
    *
@@ -286,7 +285,7 @@ public class PIDController implements Sendable, AutoCloseable {
    * @return The error.
    */
   public double getPositionError() {
-    return getContinuousError(m_positionError);
+    return m_positionError;
   }
 
   /**
@@ -315,7 +314,14 @@ public class PIDController implements Sendable, AutoCloseable {
    */
   public double calculate(double measurement) {
     m_prevError = m_positionError;
-    m_positionError = getContinuousError(m_setpoint - measurement);
+
+    if (m_continuous) {
+      m_positionError = ControllerUtil.getModulusError(m_setpoint, measurement, m_minimumInput,
+          m_maximumInput);
+    } else {
+      m_positionError = m_setpoint - measurement;
+    }
+
     m_velocityError = (m_positionError - m_prevError) / m_period;
 
     if (m_Ki != 0) {
@@ -342,42 +348,4 @@ public class PIDController implements Sendable, AutoCloseable {
     builder.addDoubleProperty("d", this::getD, this::setD);
     builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
   }
-
-  /**
-   * 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.
-   */
-  protected double getContinuousError(double error) {
-    if (m_continuous && m_inputRange > 0) {
-      error %= m_inputRange;
-      if (Math.abs(error) > m_inputRange / 2) {
-        if (error > 0) {
-          return error - m_inputRange;
-        } else {
-          return error + m_inputRange;
-        }
-      }
-    }
-    return error;
-  }
-
-  /**
-   * 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.
-   */
-  private void setInputRange(double minimumInput, double maximumInput) {
-    m_minimumInput = minimumInput;
-    m_maximumInput = maximumInput;
-    m_inputRange = maximumInput - minimumInput;
-
-    // Clamp setpoint to new input
-    if (m_maximumInput > m_minimumInput) {
-      m_setpoint = MathUtil.clamp(m_setpoint, m_minimumInput, m_maximumInput);
-    }
-  }
 }

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

@@ -23,6 +23,8 @@ public class ProfiledPIDController implements Sendable {
   private static int instances;
 
   private PIDController m_controller;
+  private double m_minimumInput;
+  private double m_maximumInput;
   private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
   private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
   private TrapezoidProfile.Constraints m_constraints;
@@ -279,6 +281,18 @@ public class ProfiledPIDController implements Sendable {
    * @param measurement The current measurement of the process variable.
    */
   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);
+
+      // 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;
+    }
+
     var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
     m_setpoint = profile.calculate(getPeriod());
     return m_controller.calculate(measurement, m_setpoint.position);

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

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ControllerUtilTest {
+  @Test
+  void testGetModulusError() {
+    // Test symmetric range
+    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));
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, 190.0, 0.0, 360.0));
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0 + 360, 0.0, 360.0));
+
+    // Test asymmetric range that doesn't start at zero
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -170.0, 190.0));
+  }
+}