From cbe05e7e8a9153a8d8456aecb7f2e3a9603349d5 Mon Sep 17 00:00:00 2001 From: Oblarg Date: Thu, 24 Oct 2019 23:37:55 -0400 Subject: [PATCH] Update ProfiledPIDController API (#1967) --- .../cpp/controller/ProfiledPIDController.cpp | 29 +++++++++++----- .../frc/controller/ProfiledPIDController.h | 25 +++++++++++--- .../ElevatorProfiledPID/cpp/Robot.cpp | 5 ++- .../examples/SwerveBot/cpp/SwerveModule.cpp | 2 +- .../controller/ProfiledPIDController.java | 34 +++++++++++++++---- 5 files changed, 72 insertions(+), 23 deletions(-) diff --git a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp index ca0ad00134..4ef3bf9a4d 100644 --- a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp +++ b/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()); + return m_controller.Calculate(measurement.to(), + m_setpoint.position.to()); } -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(); }, + "goal", [this] { return GetGoal().position.to(); }, [this](double value) { SetGoal(units::meter_t{value}); }); } diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h index a9837df8f7..64da9ebbad 100644 --- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/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); /** diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp index d025f06088..95aac42f9a 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp @@ -5,6 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +#include + #include #include #include @@ -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: diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp index 3002688aa3..697984ca6f 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp +++ b/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())); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java index ebc786f204..cc2f29dba7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java +++ b/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); } }