WPILibC++  2020.3.2-60-g3011ebe
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/ControllerUtil.h"
18 #include "frc/controller/PIDController.h"
19 #include "frc/smartdashboard/Sendable.h"
20 #include "frc/smartdashboard/SendableBuilder.h"
21 #include "frc/smartdashboard/SendableHelper.h"
22 #include "frc/trajectory/TrapezoidProfile.h"
23 
24 namespace frc {
25 namespace detail {
26 void ReportProfiledPIDController();
27 } // namespace detail
28 
33 template <class Distance>
35  : public Sendable,
36  public SendableHelper<ProfiledPIDController<Distance>> {
37  public:
38  using Distance_t = units::unit_t<Distance>;
39  using Velocity =
40  units::compound_unit<Distance, units::inverse<units::seconds>>;
41  using Velocity_t = units::unit_t<Velocity>;
42  using Acceleration =
43  units::compound_unit<Velocity, units::inverse<units::seconds>>;
44  using Acceleration_t = units::unit_t<Acceleration>;
45  using State = typename TrapezoidProfile<Distance>::State;
46  using Constraints = typename TrapezoidProfile<Distance>::Constraints;
47 
60  ProfiledPIDController(double Kp, double Ki, double Kd,
61  Constraints constraints, units::second_t period = 20_ms)
62  : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
63  detail::ReportProfiledPIDController();
64  }
65 
66  ~ProfiledPIDController() override = default;
67 
69  ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
71  ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
72 
82  void SetPID(double Kp, double Ki, double Kd) {
83  m_controller.SetPID(Kp, Ki, Kd);
84  }
85 
91  void SetP(double Kp) { m_controller.SetP(Kp); }
92 
98  void SetI(double Ki) { m_controller.SetI(Ki); }
99 
105  void SetD(double Kd) { m_controller.SetD(Kd); }
106 
112  double GetP() const { return m_controller.GetP(); }
113 
119  double GetI() const { return m_controller.GetI(); }
120 
126  double GetD() const { return m_controller.GetD(); }
127 
133  units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
134 
140  void SetGoal(State goal) { m_goal = goal; }
141 
147  void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
148 
152  State GetGoal() const { return m_goal; }
153 
159  bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
160 
166  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
167 
173  State GetSetpoint() const { return m_setpoint; }
174 
184  bool AtSetpoint() const { return m_controller.AtSetpoint(); }
185 
196  void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
197  m_controller.EnableContinuousInput(minimumInput.template to<double>(),
198  maximumInput.template to<double>());
199  }
200 
205 
215  void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
216  m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
217  }
218 
227  Distance_t positionTolerance,
228  Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
229  m_controller.SetTolerance(positionTolerance.template to<double>(),
230  velocityTolerance.template to<double>());
231  }
232 
238  Distance_t GetPositionError() const {
239  return Distance_t(m_controller.GetPositionError());
240  }
241 
245  Velocity_t GetVelocityError() const {
246  return Velocity_t(m_controller.GetVelocityError());
247  }
248 
254  double Calculate(Distance_t measurement) {
255  if (m_controller.IsContinuousInputEnabled()) {
256  // Get error which is smallest distance between goal and measurement
257  auto error = frc::GetModulusError<Distance_t>(
258  m_goal.position, measurement, m_minimumInput, m_maximumInput);
259 
260  // Recompute the profile goal with the smallest error, thus giving the
261  // shortest path. The goal may be outside the input range after this
262  // operation, but that's OK because the controller will still go there and
263  // report an error of zero. In other words, the setpoint only needs to be
264  // offset from the measurement by the input range modulus; they don't need
265  // to be equal.
266  m_goal.position = Distance_t{error} + measurement;
267  }
268 
269  frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
270  m_setpoint = profile.Calculate(GetPeriod());
271  return m_controller.Calculate(measurement.template to<double>(),
272  m_setpoint.position.template to<double>());
273  }
274 
281  double Calculate(Distance_t measurement, State goal) {
282  SetGoal(goal);
283  return Calculate(measurement);
284  }
291  double Calculate(Distance_t measurement, Distance_t goal) {
292  SetGoal(goal);
293  return Calculate(measurement);
294  }
295 
303  double Calculate(
304  Distance_t measurement, Distance_t goal,
305  typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
306  SetConstraints(constraints);
307  return Calculate(measurement, goal);
308  }
309 
315  void Reset(const State& measurement) {
316  m_controller.Reset();
317  m_setpoint = measurement;
318  }
319 
326  void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
327  Reset(State{measuredPosition, measuredVelocity});
328  }
329 
336  void Reset(Distance_t measuredPosition) {
337  Reset(measuredPosition, Velocity_t(0));
338  }
339 
340  void InitSendable(frc::SendableBuilder& builder) override {
341  builder.SetSmartDashboardType("ProfiledPIDController");
342  builder.AddDoubleProperty("p", [this] { return GetP(); },
343  [this](double value) { SetP(value); });
344  builder.AddDoubleProperty("i", [this] { return GetI(); },
345  [this](double value) { SetI(value); });
346  builder.AddDoubleProperty("d", [this] { return GetD(); },
347  [this](double value) { SetD(value); });
348  builder.AddDoubleProperty(
349  "goal", [this] { return GetGoal().position.template to<double>(); },
350  [this](double value) { SetGoal(Distance_t{value}); });
351  }
352 
353  private:
354  frc2::PIDController m_controller;
355  Distance_t m_minimumInput{0};
356  Distance_t m_maximumInput{0};
358  typename frc::TrapezoidProfile<Distance>::State m_setpoint;
359  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
360 };
361 
362 } // namespace frc
frc2::PIDController::IsContinuousInputEnabled
bool IsContinuousInputEnabled() const
Returns true if continuous input is enabled.
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:303
frc::ProfiledPIDController::SetD
void SetD(double Kd)
Sets the differential coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:105
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 setpoint.
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:340
frc::ProfiledPIDController::Reset
void Reset(Distance_t measuredPosition)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:336
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:140
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:119
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:238
frc::ProfiledPIDController::SetConstraints
void SetConstraints(Constraints constraints)
Set velocity and acceleration constraints for goal.
Definition: ProfiledPIDController.h:166
frc::ProfiledPIDController::GetSetpoint
State GetSetpoint() const
Returns the current setpoint of the ProfiledPIDController.
Definition: ProfiledPIDController.h:173
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:112
frc::ProfiledPIDController
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:34
frc::ProfiledPIDController::GetGoal
State GetGoal() const
Gets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:152
frc::ProfiledPIDController::GetPeriod
units::second_t GetPeriod() const
Gets the period of this controller.
Definition: ProfiledPIDController.h:133
frc::ProfiledPIDController::Calculate
double Calculate(Distance_t measurement)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:254
frc::ProfiledPIDController::Calculate
double Calculate(Distance_t measurement, State goal)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:281
frc::ProfiledPIDController::Calculate
double Calculate(Distance_t measurement, Distance_t goal)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:291
frc::ProfiledPIDController::EnableContinuousInput
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput)
Enables continuous input.
Definition: ProfiledPIDController.h:196
frc::ProfiledPIDController::AtSetpoint
bool AtSetpoint() const
Returns true if the error is within the tolerance of the error.
Definition: ProfiledPIDController.h:184
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:60
frc::ProfiledPIDController::SetGoal
void SetGoal(Distance_t goal)
Sets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:147
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:204
frc::ProfiledPIDController::Reset
void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:326
frc::ProfiledPIDController::SetIntegratorRange
void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
Sets the minimum and maximum values for the integrator.
Definition: ProfiledPIDController.h:215
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:159
frc
A class that enforces constraints on the differential drive kinematics.
Definition: PDPSim.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:126
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:315
frc::ProfiledPIDController::SetI
void SetI(double Ki)
Sets the integral coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:98
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:82
frc::ProfiledPIDController::SetP
void SetP(double Kp)
Sets the proportional coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:91
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:226
frc::ProfiledPIDController::GetVelocityError
Velocity_t GetVelocityError() const
Returns the change in error per second.
Definition: ProfiledPIDController.h:245
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