Update ProfiledPIDController API (#1967)

wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp

@@ -35,12 +35,16 @@ units::second_t ProfiledPIDController::GetPeriod() const {
   return m_controller.GetPeriod();
 }
 
+void ProfiledPIDController::SetGoal(TrapezoidProfile::State goal) {
+  m_goal = goal;
+}
+
 void ProfiledPIDController::SetGoal(units::meter_t goal) {
   m_goal = {goal, 0_mps};
 }
 
-units::meter_t ProfiledPIDController::GetGoal() const {
-  return m_goal.position;
+TrapezoidProfile::State ProfiledPIDController::GetGoal() const {
+  return m_goal;
 }
 
 bool ProfiledPIDController::AtGoal() const {
@@ -52,8 +56,8 @@ void ProfiledPIDController::SetConstraints(
   m_constraints = constraints;
 }
 
-double ProfiledPIDController::GetSetpoint() const {
-  return m_controller.GetSetpoint();
+TrapezoidProfile::State ProfiledPIDController::GetSetpoint() const {
+  return m_setpoint;
 }
 
 bool ProfiledPIDController::AtSetpoint() const {
@@ -87,20 +91,27 @@ double ProfiledPIDController::GetVelocityError() const {
   return m_controller.GetVelocityError();
 }
 
-double ProfiledPIDController::Calculate(double measurement) {
+double ProfiledPIDController::Calculate(units::meter_t measurement) {
   frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
   m_setpoint = profile.Calculate(GetPeriod());
-  return m_controller.Calculate(measurement, m_setpoint.position.to<double>());
+  return m_controller.Calculate(measurement.to<double>(),
+                                m_setpoint.position.to<double>());
 }
 
-double ProfiledPIDController::Calculate(double measurement,
+double ProfiledPIDController::Calculate(units::meter_t measurement,
+                                        TrapezoidProfile::State goal) {
+  SetGoal(goal);
+  return Calculate(measurement);
+}
+
+double ProfiledPIDController::Calculate(units::meter_t measurement,
                                         units::meter_t goal) {
   SetGoal(goal);
   return Calculate(measurement);
 }
 
 double ProfiledPIDController::Calculate(
-    double measurement, units::meter_t goal,
+    units::meter_t measurement, units::meter_t goal,
     frc::TrapezoidProfile::Constraints constraints) {
   SetConstraints(constraints);
   return Calculate(measurement, goal);
@@ -117,6 +128,6 @@ void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
   builder.AddDoubleProperty("d", [this] { return GetD(); },
                             [this](double value) { SetD(value); });
   builder.AddDoubleProperty(
-      "goal", [this] { return GetGoal().to<double>(); },
+      "goal", [this] { return GetGoal().position.to<double>(); },
       [this](double value) { SetGoal(units::meter_t{value}); });
 }

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

@@ -108,6 +108,13 @@ class ProfiledPIDController : public Sendable,
    */
   units::second_t GetPeriod() const;
 
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  void SetGoal(TrapezoidProfile::State goal);
+
   /**
    * Sets the goal for the ProfiledPIDController.
    *
@@ -118,7 +125,7 @@ class ProfiledPIDController : public Sendable,
   /**
    * Gets the goal for the ProfiledPIDController.
    */
-  units::meter_t GetGoal() const;
+  TrapezoidProfile::State GetGoal() const;
 
   /**
    * Returns true if the error is within the tolerance of the error.
@@ -139,7 +146,7 @@ class ProfiledPIDController : public Sendable,
    *
    * @return The current setpoint.
    */
-  double GetSetpoint() const;
+  TrapezoidProfile::State GetSetpoint() const;
 
   /**
    * Returns true if the error is within the tolerance of the error.
@@ -208,7 +215,7 @@ class ProfiledPIDController : public Sendable,
    *
    * @param measurement The current measurement of the process variable.
    */
-  double Calculate(double measurement);
+  double Calculate(units::meter_t measurement);
 
   /**
    * Returns the next output of the PID controller.
@@ -216,7 +223,15 @@ class ProfiledPIDController : public Sendable,
    * @param measurement The current measurement of the process variable.
    * @param goal The new goal of the controller.
    */
-  double Calculate(double measurement, units::meter_t goal);
+  double Calculate(units::meter_t measurement, TrapezoidProfile::State goal);
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  double Calculate(units::meter_t measurement, units::meter_t goal);
 
   /**
    * Returns the next output of the PID controller.
@@ -225,7 +240,7 @@ class ProfiledPIDController : public Sendable,
    * @param goal        The new goal of the controller.
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  double Calculate(double measurement, units::meter_t goal,
+  double Calculate(units::meter_t measurement, units::meter_t goal,
                    frc::TrapezoidProfile::Constraints constraints);
 
   /**

wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp

@@ -5,6 +5,8 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <units/units.h>
+
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
 #include <frc/PWMVictorSPX.h>
@@ -26,7 +28,8 @@ class Robot : public frc::TimedRobot {
     }
 
     // Run controller and update motor output
-    m_motor.Set(m_controller.Calculate(m_encoder.GetDistance()));
+    m_motor.Set(
+        m_controller.Calculate(units::meter_t(m_encoder.GetDistance())));
   }
 
  private:

wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp

@@ -41,7 +41,7 @@ void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
-      m_turningEncoder.Get(),
+      units::meter_t(m_turningEncoder.Get()),
       // We have to convert to the meters type here because that's what
       // ProfiledPIDController wants.
       units::meter_t(state.angle.Radians().to<double>()));

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

@@ -139,7 +139,16 @@ public class ProfiledPIDController implements Sendable {
   /**
    * Sets the goal for the ProfiledPIDController.
    *
-   * @param goal The desired unprofiled setpoint.
+   * @param goal The desired goal state.
+   */
+  public void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
+  }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired goal position.
    */
   public void setGoal(double goal) {
     m_goal = new TrapezoidProfile.State(goal, 0);
@@ -148,8 +157,8 @@ public class ProfiledPIDController implements Sendable {
   /**
    * Gets the goal for the ProfiledPIDController.
    */
-  public double getGoal() {
-    return m_goal.position;
+  public TrapezoidProfile.State getGoal() {
+    return m_goal;
   }
 
   /**
@@ -175,8 +184,8 @@ public class ProfiledPIDController implements Sendable {
    *
    * @return The current setpoint.
    */
-  public double getSetpoint() {
-    return m_controller.getSetpoint();
+  public TrapezoidProfile.State getSetpoint() {
+    return m_setpoint;
   }
 
   /**
@@ -274,6 +283,17 @@ public class ProfiledPIDController implements Sendable {
    * @param measurement The current measurement of the process variable.
    * @param goal The new goal of the controller.
    */
+  public double calculate(double measurement, TrapezoidProfile.State goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PIDController.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
   public double calculate(double measurement, double goal) {
     setGoal(goal);
     return calculate(measurement);
@@ -286,7 +306,7 @@ public class ProfiledPIDController implements Sendable {
    * @param goal        The new goal of the controller.
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  public double calculate(double measurement, double goal,
+  public double calculate(double measurement, TrapezoidProfile.State goal,
                    TrapezoidProfile.Constraints constraints) {
     setConstraints(constraints);
     return calculate(measurement, goal);
@@ -305,6 +325,6 @@ public class ProfiledPIDController implements Sendable {
     builder.addDoubleProperty("p", this::getP, this::setP);
     builder.addDoubleProperty("i", this::getI, this::setI);
     builder.addDoubleProperty("d", this::getD, this::setD);
-    builder.addDoubleProperty("goal", this::getGoal, this::setGoal);
+    builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
   }
 }