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.

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);