WPILibC++  2020.3.2
PIDController.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2018-2019 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 <functional>
11 #include <limits>
12 
13 #include <units/units.h>
14 
15 #include "frc/smartdashboard/Sendable.h"
16 #include "frc/smartdashboard/SendableHelper.h"
17 
18 namespace frc2 {
19 
24  public frc::SendableHelper<PIDController> {
25  public:
35  PIDController(double Kp, double Ki, double Kd,
36  units::second_t period = 20_ms);
37 
38  ~PIDController() override = default;
39 
40  PIDController(const PIDController&) = default;
41  PIDController& operator=(const PIDController&) = default;
42  PIDController(PIDController&&) = default;
43  PIDController& operator=(PIDController&&) = default;
44 
54  void SetPID(double Kp, double Ki, double Kd);
55 
61  void SetP(double Kp);
62 
68  void SetI(double Ki);
69 
75  void SetD(double Kd);
76 
82  double GetP() const;
83 
89  double GetI() const;
90 
96  double GetD() const;
97 
103  units::second_t GetPeriod() const;
104 
110  void SetSetpoint(double setpoint);
111 
117  double GetSetpoint() const;
118 
124  bool AtSetpoint() const;
125 
136  void EnableContinuousInput(double minimumInput, double maximumInput);
137 
141  void DisableContinuousInput();
142 
152  void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
153 
160  void SetTolerance(
161  double positionTolerance,
162  double velocityTolerance = std::numeric_limits<double>::infinity());
163 
167  double GetPositionError() const;
168 
172  double GetVelocityError() const;
173 
179  double Calculate(double measurement);
180 
187  double Calculate(double measurement, double setpoint);
188 
192  void Reset();
193 
194  void InitSendable(frc::SendableBuilder& builder) override;
195 
196  protected:
204  double GetContinuousError(double error) const;
205 
206  private:
207  // Factor for "proportional" control
208  double m_Kp;
209 
210  // Factor for "integral" control
211  double m_Ki;
212 
213  // Factor for "derivative" control
214  double m_Kd;
215 
216  // The period (in seconds) of the control loop running this controller
217  units::second_t m_period;
218 
219  double m_maximumIntegral = 1.0;
220 
221  double m_minimumIntegral = -1.0;
222 
223  // Maximum input - limit setpoint to this
224  double m_maximumInput = 0;
225 
226  // Minimum input - limit setpoint to this
227  double m_minimumInput = 0;
228 
229  // Input range - difference between maximum and minimum
230  double m_inputRange = 0;
231 
232  // Do the endpoints wrap around? eg. Absolute encoder
233  bool m_continuous = false;
234 
235  // The error at the time of the most recent call to Calculate()
236  double m_positionError = 0;
237  double m_velocityError = 0;
238 
239  // The error at the time of the second-most-recent call to Calculate() (used
240  // to compute velocity)
241  double m_prevError = 0;
242 
243  // The sum of the errors for use in the integral calc
244  double m_totalError = 0;
245 
246  // The error that is considered at setpoint.
247  double m_positionTolerance = 0.05;
248  double m_velocityTolerance = std::numeric_limits<double>::infinity();
249 
250  double m_setpoint = 0;
251 
258  void SetInputRange(double minimumInput, double maximumInput);
259 };
260 
261 } // namespace frc2
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.
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.
frc2::PIDController::InitSendable
void InitSendable(frc::SendableBuilder &builder) override
Initializes this Sendable object.
frc2::PIDController::SetSetpoint
void SetSetpoint(double setpoint)
Sets the setpoint for the PIDController.
frc2::PIDController::GetP
double GetP() const
Gets the proportional coefficient.
frc2::PIDController::SetD
void SetD(double Kd)
Sets the differential coefficient of the PID controller gain.
frc2::PIDController::GetI
double GetI() const
Gets the integral coefficient.
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.
frc2::PIDController::GetSetpoint
double GetSetpoint() const
Returns the current setpoint of the PIDController.
frc::Sendable
Interface for Sendable objects.
Definition: Sendable.h:17
frc2::PIDController::GetD
double GetD() const
Gets the differential coefficient.
frc2::PIDController::GetPeriod
units::second_t GetPeriod() const
Gets the period of this controller.
frc2::PIDController::SetIntegratorRange
void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
Sets the minimum and maximum values for the integrator.
frc2::PIDController
Implements a PID control loop.
Definition: PIDController.h:23
frc2::PIDController::GetPositionError
double GetPositionError() const
Returns the difference between the setpoint and the measurement.
frc2::PIDController::PIDController
PIDController(double Kp, double Ki, double Kd, units::second_t period=20_ms)
Allocates a PIDController with the given constants for Kp, Ki, and Kd.
frc2::PIDController::GetContinuousError
double GetContinuousError(double error) const
Wraps error around for continuous inputs.
frc::SendableHelper
A helper class for use with objects that add themselves to SendableRegistry.
Definition: SendableHelper.h:28
frc::SendableBuilder
Definition: SendableBuilder.h:23