WPILibC++  2020.3.2
ProfiledPIDController.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <algorithm>
11 #include <cmath>
12 #include <functional>
13 #include <limits>
14 
15 #include <units/units.h>
16 
17 #include "frc/controller/PIDController.h"
18 #include "frc/smartdashboard/Sendable.h"
19 #include "frc/smartdashboard/SendableBuilder.h"
20 #include "frc/smartdashboard/SendableHelper.h"
21 #include "frc/trajectory/TrapezoidProfile.h"
22 
23 namespace frc {
24 namespace detail {
25 void ReportProfiledPIDController();
26 } // namespace detail
27 
32 template <class Distance>
34  : public Sendable,
35  public SendableHelper<ProfiledPIDController<Distance>> {
36  public:
37  using Distance_t = units::unit_t<Distance>;
38  using Velocity =
39  units::compound_unit<Distance, units::inverse<units::seconds>>;
40  using Velocity_t = units::unit_t<Velocity>;
41  using Acceleration =
42  units::compound_unit<Velocity, units::inverse<units::seconds>>;
43  using Acceleration_t = units::unit_t<Acceleration>;
44  using State = typename TrapezoidProfile<Distance>::State;
45  using Constraints = typename TrapezoidProfile<Distance>::Constraints;
46 
59  ProfiledPIDController(double Kp, double Ki, double Kd,
60  Constraints constraints, units::second_t period = 20_ms)
61  : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
62  detail::ReportProfiledPIDController();
63  }
64 
65  ~ProfiledPIDController() override = default;
66 
68  ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
70  ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
71 
81  void SetPID(double Kp, double Ki, double Kd) {
82  m_controller.SetPID(Kp, Ki, Kd);
83  }
84 
90  void SetP(double Kp) { m_controller.SetP(Kp); }
91 
97  void SetI(double Ki) { m_controller.SetI(Ki); }
98 
104  void SetD(double Kd) { m_controller.SetD(Kd); }
105 
111  double GetP() const { return m_controller.GetP(); }
112 
118  double GetI() const { return m_controller.GetI(); }
119 
125  double GetD() const { return m_controller.GetD(); }
126 
132  units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
133 
139  void SetGoal(State goal) { m_goal = goal; }
140 
146  void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
147 
151  State GetGoal() const { return m_goal; }
152 
158  bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
159 
165  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
166 
172  State GetSetpoint() const { return m_setpoint; }
173 
183  bool AtSetpoint() const { return m_controller.AtSetpoint(); }
184 
195  void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
196  m_controller.EnableContinuousInput(minimumInput.template to<double>(),
197  maximumInput.template to<double>());
198  }
199 
204 
214  void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
215  m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
216  }
217 
226  Distance_t positionTolerance,
227  Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
228  m_controller.SetTolerance(positionTolerance.template to<double>(),
229  velocityTolerance.template to<double>());
230  }
231 
237  Distance_t GetPositionError() const {
238  return Distance_t(m_controller.GetPositionError());
239  }
240 
244  Velocity_t GetVelocityError() const {
245  return Velocity_t(m_controller.GetVelocityError());
246  }
247 
253  double Calculate(Distance_t measurement) {
254  frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
255  m_setpoint = profile.Calculate(GetPeriod());
256  return m_controller.Calculate(measurement.template to<double>(),
257  m_setpoint.position.template to<double>());
258  }
259 
266  double Calculate(Distance_t measurement, State goal) {
267  SetGoal(goal);
268  return Calculate(measurement);
269  }
276  double Calculate(Distance_t measurement, Distance_t goal) {
277  SetGoal(goal);
278  return Calculate(measurement);
279  }
280 
288  double Calculate(
289  Distance_t measurement, Distance_t goal,
290  typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
291  SetConstraints(constraints);
292  return Calculate(measurement, goal);
293  }
294 
300  void Reset(const State& measurement) {
301  m_controller.Reset();
302  m_setpoint = measurement;
303  }
304 
311  void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
312  Reset(State{measuredPosition, measuredVelocity});
313  }
314 
321  void Reset(Distance_t measuredPosition) {
322  Reset(measuredPosition, Velocity_t(0));
323  }
324 
325  void InitSendable(frc::SendableBuilder& builder) override {
326  builder.SetSmartDashboardType("ProfiledPIDController");
327  builder.AddDoubleProperty("p", [this] { return GetP(); },
328  [this](double value) { SetP(value); });
329  builder.AddDoubleProperty("i", [this] { return GetI(); },
330  [this](double value) { SetI(value); });
331  builder.AddDoubleProperty("d", [this] { return GetD(); },
332  [this](double value) { SetD(value); });
333  builder.AddDoubleProperty(
334  "goal", [this] { return GetGoal().position.template to<double>(); },
335  [this](double value) { SetGoal(Distance_t{value}); });
336  }
337 
338  private:
339  frc2::PIDController m_controller;
341  typename frc::TrapezoidProfile<Distance>::State m_setpoint;
342  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
343 };
344 
345 } // namespace frc
frc2::PIDController::SetP
void SetP(double Kp)
Sets the proportional coefficient of the PID controller gain.
frc2::PIDController::DisableContinuousInput
void DisableContinuousInput()
Disables continuous input.
frc2::PIDController::Calculate
double Calculate(double measurement)
Returns the next output of the PID controller.
frc2::PIDController::Reset
void Reset()
Reset the previous error, the integral term, and disable the controller.
frc2::PIDController::EnableContinuousInput
void EnableContinuousInput(double minimumInput, double maximumInput)
Enables continuous input.
frc::ProfiledPIDController::Calculate
double Calculate(Distance_t measurement, Distance_t goal, typename frc::TrapezoidProfile< Distance >::Constraints constraints)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:288
frc::ProfiledPIDController::SetD
void SetD(double Kd)
Sets the differential coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:104
frc2::PIDController::SetTolerance
void SetTolerance(double positionTolerance, double velocityTolerance=std::numeric_limits< double >::infinity())
Sets the error which is considered tolerable for use with AtSetpoint().
frc2::PIDController::AtSetpoint
bool AtSetpoint() const
Returns true if the error is within the tolerance of the error.
frc2::PIDController::SetI
void SetI(double Ki)
Sets the integral coefficient of the PID controller gain.
frc::ProfiledPIDController::InitSendable
void InitSendable(frc::SendableBuilder &builder) override
Initializes this Sendable object.
Definition: ProfiledPIDController.h:325
frc::ProfiledPIDController::Reset
void Reset(Distance_t measuredPosition)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:321
frc::TrapezoidProfile
A trapezoid-shaped velocity profile.
Definition: TrapezoidProfile.h:45
frc2::PIDController::GetP
double GetP() const
Gets the proportional coefficient.
frc::ProfiledPIDController::SetGoal
void SetGoal(State goal)
Sets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:139
frc2::PIDController::SetD
void SetD(double Kd)
Sets the differential coefficient of the PID controller gain.
frc::ProfiledPIDController::GetI
double GetI() const
Gets the integral coefficient.
Definition: ProfiledPIDController.h:118
frc2::PIDController::GetI
double GetI() const
Gets the integral coefficient.
frc::ProfiledPIDController::GetPositionError
Distance_t GetPositionError() const
Returns the difference between the setpoint and the measurement.
Definition: ProfiledPIDController.h:237
frc::ProfiledPIDController::SetConstraints
void SetConstraints(Constraints constraints)
Set velocity and acceleration constraints for goal.
Definition: ProfiledPIDController.h:165
frc::ProfiledPIDController::GetSetpoint
State GetSetpoint() const
Returns the current setpoint of the ProfiledPIDController.
Definition: ProfiledPIDController.h:172
frc2::PIDController::SetPID
void SetPID(double Kp, double Ki, double Kd)
Sets the PID Controller gain parameters.
frc2::PIDController::GetVelocityError
double GetVelocityError() const
Returns the velocity error.
frc::ProfiledPIDController::GetP
double GetP() const
Gets the proportional coefficient.
Definition: ProfiledPIDController.h:111
frc::ProfiledPIDController
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:33
frc::ProfiledPIDController::GetGoal
State GetGoal() const
Gets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:151
frc::ProfiledPIDController::GetPeriod
units::second_t GetPeriod() const
Gets the period of this controller.
Definition: ProfiledPIDController.h:132
frc::ProfiledPIDController::Calculate
double Calculate(Distance_t measurement)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:253
frc::ProfiledPIDController::Calculate
double Calculate(Distance_t measurement, State goal)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:266
frc::ProfiledPIDController::Calculate
double Calculate(Distance_t measurement, Distance_t goal)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:276
frc::ProfiledPIDController::EnableContinuousInput
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput)
Enables continuous input.
Definition: ProfiledPIDController.h:195
frc::ProfiledPIDController::AtSetpoint
bool AtSetpoint() const
Returns true if the error is within the tolerance of the error.
Definition: ProfiledPIDController.h:183
frc::ProfiledPIDController::ProfiledPIDController
ProfiledPIDController(double Kp, double Ki, double Kd, Constraints constraints, units::second_t period=20_ms)
Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
Definition: ProfiledPIDController.h:59
frc::ProfiledPIDController::SetGoal
void SetGoal(Distance_t goal)
Sets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:146
frc::Sendable
Interface for Sendable objects.
Definition: Sendable.h:17
frc2::PIDController::GetD
double GetD() const
Gets the differential coefficient.
frc::TrapezoidProfile::Constraints
Definition: TrapezoidProfile.h:55
frc2::PIDController::GetPeriod
units::second_t GetPeriod() const
Gets the period of this controller.
frc::ProfiledPIDController::DisableContinuousInput
void DisableContinuousInput()
Disables continuous input.
Definition: ProfiledPIDController.h:203
frc::ProfiledPIDController::Reset
void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:311
frc::ProfiledPIDController::SetIntegratorRange
void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
Sets the minimum and maximum values for the integrator.
Definition: ProfiledPIDController.h:214
frc2::PIDController::SetIntegratorRange
void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
Sets the minimum and maximum values for the integrator.
frc::TrapezoidProfile::State
Definition: TrapezoidProfile.h:68
frc::ProfiledPIDController::AtGoal
bool AtGoal() const
Returns true if the error is within the tolerance of the error.
Definition: ProfiledPIDController.h:158
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc2::PIDController
Implements a PID control loop.
Definition: PIDController.h:23
frc::ProfiledPIDController::GetD
double GetD() const
Gets the differential coefficient.
Definition: ProfiledPIDController.h:125
frc2::PIDController::GetPositionError
double GetPositionError() const
Returns the difference between the setpoint and the measurement.
frc::ProfiledPIDController::Reset
void Reset(const State &measurement)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:300
frc::ProfiledPIDController::SetI
void SetI(double Ki)
Sets the integral coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:97
frc::SendableBuilder::AddDoubleProperty
virtual void AddDoubleProperty(const wpi::Twine &key, std::function< double()> getter, std::function< void(double)> setter)=0
Add a double property.
frc::ProfiledPIDController::SetPID
void SetPID(double Kp, double Ki, double Kd)
Sets the PID Controller gain parameters.
Definition: ProfiledPIDController.h:81
frc::ProfiledPIDController::SetP
void SetP(double Kp)
Sets the proportional coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:90
frc::ProfiledPIDController::SetTolerance
void SetTolerance(Distance_t positionTolerance, Velocity_t velocityTolerance=std::numeric_limits< double >::infinity())
Sets the error which is considered tolerable for use with AtSetpoint().
Definition: ProfiledPIDController.h:225
frc::ProfiledPIDController::GetVelocityError
Velocity_t GetVelocityError() const
Returns the change in error per second.
Definition: ProfiledPIDController.h:244
frc::SendableBuilder::SetSmartDashboardType
virtual void SetSmartDashboardType(const wpi::Twine &type)=0
Set the string representation of the named data type that will be used by the smart dashboard for thi...
frc::SendableHelper
A helper class for use with objects that add themselves to SendableRegistry.
Definition: SendableHelper.h:28
frc::SendableBuilder
Definition: SendableBuilder.h:23