Add function to get next position-velocity pair from ProfiledPIDController

[calcmogul]

Is your feature request related to a problem? Please describe.
The feedforward calculate() overloads that take nextVelocity are more accurate for discrete systems. ProfiledPIDController doesn't provide a way to get it though.

Describe the solution you'd like
Add a function that returns the next setpoint pair.

[PaarkG]

First time attempting to contribute, this looks like a good spot.

A ProfiledPIDController object looks to store a current trapezoid state setpoint, which is accessible with a getter. What would this function do that is different than calculating the next trapezoid state setpoint so that the user can get the next velocity? In my mind at least this can just be achieved by running calculate() and then calling getSetpoint() to find the next velocity, so I think I'm missing something.

[auscompgeek]

calculate() has side effects. Calling it in a getter will subtly break any system using the I term.

[PaarkG]

So would this new function use the internal trapezoid profile to calculate the velocity .02 seconds (or other loop time) ahead of the current setpoint, which would technically be .04 seconds ahead?

[calcmogul]

Why would it be 0.04 seconds ahead?

[PaarkG]

I'm just confused on what the new function needs to expose. The setpoint is already accessible through getSetpoint() and if the user has already run calculate in the loop then getSetpoint() should return an updated velocity setpoint to be used for the feedforward? Is the new function just supposed to recalculate that next setpoint to make sure it is up to date without the user needing to run calculate() separately?

[calcmogul]

Here's an example, though it doesn't include the required logic for continuous inputs.

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
index 6c8cd6ea2..76c3d9d09 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
@@ -261,6 +261,15 @@ public class ProfiledPIDController implements Sendable {
     return m_setpoint;
   }
 
+  /**
+   * Returns the next setpoint of the ProfiledPIDController.
+   *
+   * @return The next setpoint.
+   */
+  public TrapezoidProfile.State getNextSetpoint() {
+    return m_profile.calculate(m_controller.getPeriod(), m_setpoint, m_goal);
+  }
+
   /**
    * Returns true if the error is within the tolerance of the error.
    *
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index a71738223..ce37e0c87 100644
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -238,6 +238,15 @@ class ProfiledPIDController
    */
   constexpr State GetSetpoint() const { return m_setpoint; }
 
+  /**
+   * Returns the next setpoint of the ProfiledPIDController.
+   *
+   * @return The next setpoint.
+   */
+  constexpr State GetNextSetpoint() const {
+    return m_profile.Calculate(m_controller.GetPeriod(), m_setpoint, m_goal);
+  }
+
   /**
    * Returns true if the error is within the tolerance of the error.
    *

[PaarkG]

Thank you, I understand now. I'll see if I can put something together with continuous input support.