From 8f402645f5070ee210ef5f92f467be3d2101683b Mon Sep 17 00:00:00 2001
From: sciencewhiz <sciencewhiz@users.noreply.github.com>
Date: Sat, 3 Dec 2022 11:32:46 -0800
Subject: [PATCH] [commands] Fix PIDSubsystem setSetpoint behavior (#4759)

No longer stores a temporary setpoint in PIDSubsystem, instead
immediately sending to PIDController. This fixes an issue where the
setpoint didn't take effect until the Subsystem Periodic method ran, and
could cause commands to finish early if they were scheduled after the
subsystem periodic method ran because it used the old setpoint.
---
 .../edu/wpi/first/wpilibj2/command/PIDSubsystem.java   | 10 ++++------
 .../src/main/native/cpp/frc2/command/PIDSubsystem.cpp  |  6 +++---
 .../main/native/include/frc2/command/PIDSubsystem.h    |  3 ---
 3 files changed, 7 insertions(+), 12 deletions(-)

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
index c15360d29c..32dbb0a790 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
@@ -18,8 +18,6 @@ public abstract class PIDSubsystem extends SubsystemBase {
   protected final PIDController m_controller;
   protected boolean m_enabled;
 
-  private double m_setpoint;
-
   /**
    * Creates a new PIDSubsystem.
    *
@@ -27,8 +25,8 @@ public abstract class PIDSubsystem extends SubsystemBase {
    * @param initialPosition the initial setpoint of the subsystem
    */
   public PIDSubsystem(PIDController controller, double initialPosition) {
-    setSetpoint(initialPosition);
     m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
+    setSetpoint(initialPosition);
     addChild("PID Controller", m_controller);
   }
 
@@ -44,7 +42,7 @@ public abstract class PIDSubsystem extends SubsystemBase {
   @Override
   public void periodic() {
     if (m_enabled) {
-      useOutput(m_controller.calculate(getMeasurement(), m_setpoint), m_setpoint);
+      useOutput(m_controller.calculate(getMeasurement()), getSetpoint());
     }
   }
 
@@ -58,7 +56,7 @@ public abstract class PIDSubsystem extends SubsystemBase {
    * @param setpoint the setpoint for the subsystem
    */
   public void setSetpoint(double setpoint) {
-    m_setpoint = setpoint;
+    m_controller.setSetpoint(setpoint);
   }
 
   /**
@@ -67,7 +65,7 @@ public abstract class PIDSubsystem extends SubsystemBase {
    * @return The current setpoint
    */
   public double getSetpoint() {
-    return m_setpoint;
+    return m_controller.getSetpoint();
   }
 
   /**
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
index 7c820aef96..8db032a720 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
@@ -16,16 +16,16 @@ PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition)
 
 void PIDSubsystem::Periodic() {
   if (m_enabled) {
-    UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint), m_setpoint);
+    UseOutput(m_controller.Calculate(GetMeasurement()), GetSetpoint());
   }
 }
 
 void PIDSubsystem::SetSetpoint(double setpoint) {
-  m_setpoint = setpoint;
+  m_controller.SetSetpoint(setpoint);
 }
 
 double PIDSubsystem::GetSetpoint() const {
-  return m_setpoint;
+  return m_controller.GetSetpoint();
 }
 
 void PIDSubsystem::Enable() {
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
index 37571adc2f..426e3ecc63 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -85,8 +85,5 @@ class PIDSubsystem : public SubsystemBase {
    * @param setpoint the setpoint of the PIDController (for feedforward)
    */
   virtual void UseOutput(double output, double setpoint) = 0;
-
- private:
-  double m_setpoint{0};
 };
 }  // namespace frc2