Fix trapezoidal profile PID controller setpoint bug (#2210)

Co-Authored-By: Austin Shalit <austinshalit@gmail.com>

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java

@@ -113,7 +113,7 @@ public class ProfiledPIDCommand extends CommandBase {
 
   @Override
   public void initialize() {
-    m_controller.reset();
+    m_controller.reset(m_measurement.getAsDouble());
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java

@@ -93,7 +93,7 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
    */
   public void enable() {
     m_enabled = true;
-    m_controller.reset();
+    m_controller.reset(getMeasurement());
   }
 
   /**

wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h

@@ -118,7 +118,7 @@ class ProfiledPIDCommand
 
   ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
 
-  void Initialize() override { m_controller.Reset(); }
+  void Initialize() override { m_controller.Reset(m_measurement()); }
 
   void Execute() override {
     m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),

wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h

@@ -62,10 +62,10 @@ class ProfiledPIDSubsystem : public SubsystemBase {
   void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
 
   /**
-   * Enables the PID control.  Resets the controller.
+   * Enables the PID control. Resets the controller.
    */
   virtual void Enable() {
-    m_controller.Reset();
+    m_controller.Reset(GetMeasurement());
     m_enabled = true;
   }
 

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-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.                                                               */
@@ -46,7 +46,8 @@ class ProfiledPIDController
  public:
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
-   * Kd.
+   * Kd. Users should call reset() when they first start running the controller
+   * to avoid unwanted behavior.
    *
    * @param Kp          The proportional coefficient.
    * @param Ki          The integral coefficient.
@@ -292,9 +293,34 @@ class ProfiledPIDController
   }
 
   /**
-   * Reset the previous error, the integral term, and disable the controller.
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
    */
-  void Reset() { m_controller.Reset(); }
+  void Reset(const State& measurement) {
+    m_controller.Reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
+    Reset(State{measuredPosition, measuredVelocity});
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The
+   * velocity is assumed to be zero.
+   */
+  void Reset(Distance_t measuredPosition) {
+    Reset(measuredPosition, Velocity_t(0));
+  }
 
   void InitSendable(frc::SendableBuilder& builder) override {
     builder.SetSmartDashboardType("ProfiledPIDController");

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

@@ -328,7 +328,7 @@ public class PIDController implements Sendable, AutoCloseable {
   }
 
   /**
-   * Resets the previous error and the integral term. Also disables the controller.
+   * Resets the previous error and the integral term.
    */
   public void reset() {
     m_prevError = 0;

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-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.                                                               */
@@ -15,7 +15,8 @@ import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
 
 /**
  * Implements a PID control loop whose setpoint is constrained by a trapezoid
- * profile.
+ * profile. Users should call reset() when they first start running the controller
+ * to avoid unwanted behavior.
  */
 @SuppressWarnings("PMD.TooManyMethods")
 public class ProfiledPIDController implements Sendable {
@@ -319,10 +320,33 @@ public class ProfiledPIDController implements Sendable {
   }
 
   /**
-   * Reset the previous error, the integral term, and disable the controller.
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
    */
-  public void reset() {
+  public void reset(TrapezoidProfile.State measurement) {
     m_controller.reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  public void reset(double measuredPosition, double measuredVelocity) {
+    reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The velocity is
+   *     assumed to be zero.
+   */
+  public void reset(double measuredPosition) {
+    reset(measuredPosition, 0.0);
   }
 
   @Override

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

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ProfiledPIDControllerTest {
+  @Test
+  void testStartFromNonZeroPosition() {
+    ProfiledPIDController controller = new ProfiledPIDController(1.0, 0.0, 0.0,
+        new TrapezoidProfile.Constraints(1.0, 1.0));
+
+    controller.reset(20);
+
+    assertEquals(0.0, controller.calculate(20), 0.05);
+  }
+}