32template <
class Distance>
60 Constraints constraints, units::second_t period = 20_ms)
61 : m_controller{Kp, Ki, Kd, period},
62 m_constraints{constraints},
63 m_profile{m_constraints} {
86 void SetPID(
double Kp,
double Ki,
double Kd) {
87 m_controller.
SetPID(Kp, Ki, Kd);
95 void SetP(
double Kp) { m_controller.
SetP(Kp); }
128 double GetP()
const {
return m_controller.
GetP(); }
135 double GetI()
const {
return m_controller.
GetI(); }
142 double GetD()
const {
return m_controller.
GetD(); }
208 m_constraints = constraints;
248 maximumInput.
value());
249 m_minimumInput = minimumInput;
250 m_maximumInput = maximumInput;
280 std::numeric_limits<double>::infinity()}) {
282 velocityTolerance.value());
309 auto errorBound = (m_maximumInput - m_minimumInput) / 2.0;
310 auto goalMinDistance = frc::InputModulus<Distance_t>(
311 m_goal.position - measurement, -errorBound, errorBound);
312 auto setpointMinDistance = frc::InputModulus<Distance_t>(
313 m_setpoint.position - measurement, -errorBound, errorBound);
321 m_goal.position = goalMinDistance + measurement;
322 m_setpoint.position = setpointMinDistance + measurement;
325 m_setpoint = m_profile.Calculate(
GetPeriod(), m_setpoint, m_goal);
327 m_setpoint.position.value());
371 m_controller.
Reset();
372 m_setpoint = measurement;
382 Reset(
State{measuredPosition, measuredVelocity});
398 "p", [
this] {
return GetP(); }, [
this](
double value) {
SetP(value); });
400 "i", [
this] {
return GetI(); }, [
this](
double value) {
SetI(value); });
402 "d", [
this] {
return GetD(); }, [
this](
double value) {
SetD(value); });
404 "izone", [
this] {
return GetIZone(); },
405 [
this](
double value) {
SetIZone(value); });
407 "goal", [
this] {
return GetGoal().position.value(); },
417 TrapezoidProfile<Distance> m_profile;
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Implements a PID control loop.
Definition: PIDController.h:23
double GetPositionError() const
Returns the difference between the setpoint and the measurement.
double GetVelocityTolerance() const
Gets the velocity tolerance of this controller.
void SetPID(double Kp, double Ki, double Kd)
Sets the PID Controller gain parameters.
bool IsContinuousInputEnabled() const
Returns true if continuous input is enabled.
units::second_t GetPeriod() const
Gets the period of this controller.
double GetIZone() const
Get the IZone range.
double GetP() const
Gets the proportional coefficient.
void DisableContinuousInput()
Disables continuous input.
double GetD() const
Gets the differential coefficient.
void SetI(double Ki)
Sets the integral coefficient of the PID controller gain.
void SetIZone(double iZone)
Sets the IZone range.
void SetP(double Kp)
Sets the proportional coefficient of the PID controller gain.
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.
double GetPositionTolerance() const
Gets the position tolerance of this controller.
double GetI() const
Gets the integral coefficient.
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 setpoint.
void SetD(double Kd)
Sets the differential coefficient of the PID controller gain.
void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
Sets the minimum and maximum values for the integrator.
void EnableContinuousInput(double minimumInput, double maximumInput)
Enables continuous input.
double GetVelocityError() const
Returns the velocity error.
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:35
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
double GetP() const
Gets the proportional coefficient.
Definition: ProfiledPIDController.h:128
State GetSetpoint() const
Returns the current setpoint of the ProfiledPIDController.
Definition: ProfiledPIDController.h:223
double Calculate(Distance_t measurement, Distance_t goal)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:346
ProfiledPIDController & operator=(ProfiledPIDController &&)=default
void Reset(Distance_t measuredPosition)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:391
State GetGoal() const
Gets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:193
bool AtSetpoint() const
Returns true if the error is within the tolerance of the error.
Definition: ProfiledPIDController.h:234
double Calculate(Distance_t measurement, State goal)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:336
units::unit_t< Distance > Distance_t
Definition: ProfiledPIDController.h:37
typename TrapezoidProfile< Distance >::Constraints Constraints
Definition: ProfiledPIDController.h:45
Distance_t GetPositionError() const
Returns the difference between the setpoint and the measurement.
Definition: ProfiledPIDController.h:290
void SetTolerance(Distance_t positionTolerance, Velocity_t velocityTolerance=Velocity_t{ std::numeric_limits< double >::infinity()})
Sets the error which is considered tolerable for use with AtSetpoint().
Definition: ProfiledPIDController.h:278
void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
Sets the minimum and maximum values for the integrator.
Definition: ProfiledPIDController.h:267
void SetI(double Ki)
Sets the integral coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:102
ProfiledPIDController(const ProfiledPIDController &)=default
void SetGoal(Distance_t goal)
Sets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:188
void DisableContinuousInput()
Disables continuous input.
Definition: ProfiledPIDController.h:256
void SetD(double Kd)
Sets the differential coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:109
units::second_t GetPeriod() const
Gets the period of this controller.
Definition: ProfiledPIDController.h:156
Velocity_t GetVelocityError() const
Returns the change in error per second.
Definition: ProfiledPIDController.h:297
double GetD() const
Gets the differential coefficient.
Definition: ProfiledPIDController.h:142
void SetP(double Kp)
Sets the proportional coefficient of the PID controller gain.
Definition: ProfiledPIDController.h:95
double GetPositionTolerance() const
Gets the position tolerance of this controller.
Definition: ProfiledPIDController.h:163
double GetIZone() const
Get the IZone range.
Definition: ProfiledPIDController.h:149
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:358
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput)
Enables continuous input.
Definition: ProfiledPIDController.h:246
ProfiledPIDController(ProfiledPIDController &&)=default
double GetVelocityTolerance() const
Gets the velocity tolerance of this controller.
Definition: ProfiledPIDController.h:172
void InitSendable(wpi::SendableBuilder &builder) override
Initializes this Sendable object.
Definition: ProfiledPIDController.h:395
ProfiledPIDController & operator=(const ProfiledPIDController &)=default
units::compound_unit< Velocity, units::inverse< units::seconds > > Acceleration
Definition: ProfiledPIDController.h:42
typename TrapezoidProfile< Distance >::State State
Definition: ProfiledPIDController.h:44
void SetPID(double Kp, double Ki, double Kd)
Sets the PID Controller gain parameters.
Definition: ProfiledPIDController.h:86
void Reset(const State &measurement)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:370
units::compound_unit< Distance, units::inverse< units::seconds > > Velocity
Definition: ProfiledPIDController.h:39
double Calculate(Distance_t measurement)
Returns the next output of the PID controller.
Definition: ProfiledPIDController.h:306
void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity)
Reset the previous error and the integral term.
Definition: ProfiledPIDController.h:381
void SetGoal(State goal)
Sets the goal for the ProfiledPIDController.
Definition: ProfiledPIDController.h:181
double GetI() const
Gets the integral coefficient.
Definition: ProfiledPIDController.h:135
Constraints GetConstraints()
Get the velocity and acceleration constraints for this controller.
Definition: ProfiledPIDController.h:216
~ProfiledPIDController() override=default
void SetConstraints(Constraints constraints)
Set velocity and acceleration constraints for goal.
Definition: ProfiledPIDController.h:207
bool AtGoal() const
Returns true if the error is within the tolerance of the error.
Definition: ProfiledPIDController.h:200
void SetIZone(double iZone)
Sets the IZone range.
Definition: ProfiledPIDController.h:121
Definition: TrapezoidProfile.h:55
Definition: TrapezoidProfile.h:70
A trapezoid-shaped velocity profile.
Definition: TrapezoidProfile.h:45
constexpr underlying_type value() const noexcept
unit value
Definition: base.h:2119
Definition: SendableBuilder.h:18
virtual void AddDoubleProperty(std::string_view key, std::function< double()> getter, std::function< void(double)> setter)=0
Add a double property.
virtual void SetSmartDashboardType(std::string_view 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:19
Interface for Sendable objects.
Definition: Sendable.h:16
static void Add(Sendable *sendable, std::string_view name)
Adds an object to the registry.
static void ReportUsage(MathUsageId id, int count)
Definition: MathShared.h:73
typename units::detail::compound_impl< U, Us... >::type compound_unit
Represents a unit type made up from other units.
Definition: base.h:1446
detail namespace with internal helper functions
Definition: ranges.h:23
WPILIB_DLLEXPORT int IncrementAndGetProfiledPIDControllerInstances()
Definition: AprilTagPoseEstimator.h:15
@ kController_ProfiledPIDController