15 #include <units/units.h>
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"
25 void ReportProfiledPIDController();
32 template <
class Distance>
37 using Distance_t = units::unit_t<Distance>;
39 units::compound_unit<Distance, units::inverse<units::seconds>>;
40 using Velocity_t = units::unit_t<Velocity>;
42 units::compound_unit<Velocity, units::inverse<units::seconds>>;
43 using Acceleration_t = units::unit_t<Acceleration>;
60 Constraints constraints, units::second_t period = 20_ms)
61 : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
62 detail::ReportProfiledPIDController();
81 void SetPID(
double Kp,
double Ki,
double Kd) {
82 m_controller.
SetPID(Kp, Ki, Kd);
90 void SetP(
double Kp) { m_controller.
SetP(Kp); }
97 void SetI(
double Ki) { m_controller.
SetI(Ki); }
111 double GetP()
const {
return m_controller.
GetP(); }
118 double GetI()
const {
return m_controller.
GetI(); }
125 double GetD()
const {
return m_controller.
GetD(); }
146 void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
197 maximumInput.template to<double>());
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>());
255 m_setpoint = profile.Calculate(
GetPeriod());
256 return m_controller.
Calculate(measurement.template to<double>(),
257 m_setpoint.position.template to<double>());
276 double Calculate(Distance_t measurement, Distance_t goal) {
289 Distance_t measurement, Distance_t goal,
300 void Reset(
const State& measurement) {
301 m_controller.
Reset();
302 m_setpoint = measurement;
311 void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
312 Reset(State{measuredPosition, measuredVelocity});
321 void Reset(Distance_t measuredPosition) {
322 Reset(measuredPosition, Velocity_t(0));
328 [
this](
double value) {
SetP(value); });
330 [
this](
double value) {
SetI(value); });
332 [
this](
double value) {
SetD(value); });
334 "goal", [
this] {
return GetGoal().position.template to<double>(); },
335 [
this](
double value) {
SetGoal(Distance_t{value}); });
void SetP(double Kp)
Sets the proportional coefficient of the PID controller gain.
void DisableContinuousInput()
Disables continuous input.
double Calculate(double measurement)
Returns the next output of the PID controller.
void Reset()
Reset the previous error, the integral term, and disable the controller.
void EnableContinuousInput(double minimumInput, double maximumInput)
Enables continuous input.
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
void SetD(double Kd)
Sets the differential coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:104
void SetTolerance(double positionTolerance, double velocityTolerance=std::numeric_limits< double >::infinity())
Sets the error which is considered tolerable for use with AtSetpoint().
bool AtSetpoint() const
Returns true if the error is within the tolerance of the error.
void SetI(double Ki)
Sets the integral coefficient of the PID controller gain.
void InitSendable(frc::SendableBuilder &builder) override
Initializes this Sendable object.
Definition: ProfiledPIDController.h:325
void Reset(Distance_t measuredPosition)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:321
A trapezoid-shaped velocity profile.
Definition: TrapezoidProfile.h:45
double GetP() const
Gets the proportional coefficient.
void SetGoal(State goal)
Sets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:139
void SetD(double Kd)
Sets the differential coefficient of the PID controller gain.
double GetI() const
Gets the integral coefficient.
Definition: ProfiledPIDController.h:118
double GetI() const
Gets the integral coefficient.
Distance_t GetPositionError() const
Returns the difference between the setpoint and the measurement.
Definition: ProfiledPIDController.h:237
void SetConstraints(Constraints constraints)
Set velocity and acceleration constraints for goal.
Definition: ProfiledPIDController.h:165
State GetSetpoint() const
Returns the current setpoint of the ProfiledPIDController.
Definition: ProfiledPIDController.h:172
void SetPID(double Kp, double Ki, double Kd)
Sets the PID Controller gain parameters.
double GetVelocityError() const
Returns the velocity error.
double GetP() const
Gets the proportional coefficient.
Definition: ProfiledPIDController.h:111
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:33
State GetGoal() const
Gets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:151
units::second_t GetPeriod() const
Gets the period of this controller.
Definition: ProfiledPIDController.h:132
double Calculate(Distance_t measurement)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:253
double Calculate(Distance_t measurement, State goal)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:266
double Calculate(Distance_t measurement, Distance_t goal)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:276
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput)
Enables continuous input.
Definition: ProfiledPIDController.h:195
bool AtSetpoint() const
Returns true if the error is within the tolerance of the error.
Definition: ProfiledPIDController.h:183
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
void SetGoal(Distance_t goal)
Sets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:146
Interface for Sendable objects.
Definition: Sendable.h:17
double GetD() const
Gets the differential coefficient.
Definition: TrapezoidProfile.h:55
units::second_t GetPeriod() const
Gets the period of this controller.
void DisableContinuousInput()
Disables continuous input.
Definition: ProfiledPIDController.h:203
void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:311
void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
Sets the minimum and maximum values for the integrator.
Definition: ProfiledPIDController.h:214
void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
Sets the minimum and maximum values for the integrator.
Definition: TrapezoidProfile.h:68
bool AtGoal() const
Returns true if the error is within the tolerance of the error.
Definition: ProfiledPIDController.h:158
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
Implements a PID control loop.
Definition: PIDController.h:23
double GetD() const
Gets the differential coefficient.
Definition: ProfiledPIDController.h:125
double GetPositionError() const
Returns the difference between the setpoint and the measurement.
void Reset(const State &measurement)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:300
void SetI(double Ki)
Sets the integral coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:97
virtual void AddDoubleProperty(const wpi::Twine &key, std::function< double()> getter, std::function< void(double)> setter)=0
Add a double property.
void SetPID(double Kp, double Ki, double Kd)
Sets the PID Controller gain parameters.
Definition: ProfiledPIDController.h:81
void SetP(double Kp)
Sets the proportional coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:90
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
Velocity_t GetVelocityError() const
Returns the change in error per second.
Definition: ProfiledPIDController.h:244
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...
A helper class for use with objects that add themselves to SendableRegistry.
Definition: SendableHelper.h:28
Definition: SendableBuilder.h:23