13 #include <units/units.h>
15 #include "frc/smartdashboard/Sendable.h"
16 #include "frc/smartdashboard/SendableHelper.h"
36 units::second_t period = 20_ms);
54 void SetPID(
double Kp,
double Ki,
double Kd);
161 double positionTolerance,
162 double velocityTolerance = std::numeric_limits<double>::infinity());
187 double Calculate(
double measurement,
double setpoint);
217 units::second_t m_period;
219 double m_maximumIntegral = 1.0;
221 double m_minimumIntegral = -1.0;
224 double m_maximumInput = 0;
227 double m_minimumInput = 0;
230 double m_inputRange = 0;
233 bool m_continuous =
false;
236 double m_positionError = 0;
237 double m_velocityError = 0;
241 double m_prevError = 0;
244 double m_totalError = 0;
247 double m_positionTolerance = 0.05;
248 double m_velocityTolerance = std::numeric_limits<double>::infinity();
250 double m_setpoint = 0;
258 void SetInputRange(
double minimumInput,
double maximumInput);
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.
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.
void SetSetpoint(double setpoint)
Sets the setpoint for the PIDController.
double GetP() const
Gets the proportional coefficient.
void SetD(double Kd)
Sets the differential coefficient of the PID controller gain.
double GetI() const
Gets the integral coefficient.
void SetPID(double Kp, double Ki, double Kd)
Sets the PID Controller gain parameters.
double GetVelocityError() const
Returns the velocity error.
double GetSetpoint() const
Returns the current setpoint of the PIDController.
Interface for Sendable objects.
Definition: Sendable.h:17
double GetD() const
Gets the differential coefficient.
units::second_t GetPeriod() const
Gets the period of this controller.
void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
Sets the minimum and maximum values for the integrator.
Implements a PID control loop.
Definition: PIDController.h:23
double GetPositionError() const
Returns the difference between the setpoint and the measurement.
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.
double GetContinuousError(double error) const
Wraps error around for continuous inputs.
A helper class for use with objects that add themselves to SendableRegistry.
Definition: SendableHelper.h:28
Definition: SendableBuilder.h:23