PID Control in WPILib¶
This article covers the in-code implementation of PID Control with WPILib’s provided library classes. Documentation describing the involved concepts in more detail is forthcoming.
WPILib supports PID control of mechanisms through the
PIDController class (Java, C++). This class handles the feedback loop calculation for the user, as well as offering methods for returning the error, setting tolerances, and checking if the control loop has reached its setpoint within the specified tolerances.
Using the PIDController Class¶
PIDController class in the
frc namespace is deprecated - C++ teams should use the one in the
frc2 namespace, instead. Likewise, Java teams should use the class in the
Constructing a PIDController¶
PIDController may be used asynchronously, it does not provide any thread safety features - ensuring threadsafe operation is left entirely to the user, and thus asynchronous usage is recommended only for advanced teams.
In order to use WPILib’s PID control functionality, users must first construct a
PIDController object with the desired gains:
An optional fourth parameter can be provided to the constructor, specifying the period at which the controller will be run. The
PIDController object is intended primarily for synchronous use from the main robot loop, and so this value is defaulted to 20ms.
Using the Feedback Loop Output¶
PIDController assumes that the
calculate() method is being called regularly at an interval consistent with the configured period. Failure to do this will result in unintended loop behavior.
Unlike the old
PIDController, the new PIDController does not automatically control an output from its own thread - users are required to call
calculate() and use the resulting output in their own code.
Using the constructed
PIDController is simple: simply call the
calculate() method from the robot’s main loop (e.g. the robot’s
getVelocityError() are named assuming that the loop is controlling a position - for a loop that is controlling a velocity, these return the velocity error and the acceleration error, respectively.
If using continuous input, use
getContinuousError rather than
getPositionError. Velocity error is unaffected.
The current error of the measured process variable is returned by the
getPositionError() function, while its derivative is returned by the
Specifying and Checking Tolerances¶
If only a position tolerance is specified, the velocity tolerance defaults to infinity.
As above, “position” refers to the process variable measurement, and “velocity” to its derivative - thus, for a velocity loop, these are actually velocity and acceleration, respectively.
Occasionally, it is useful to know if a controller has tracked the setpoint to within a given tolerance - for example, to determine if a command should be ended, or (while following a motion profile) if motion is being impeded and needs to be re-planned.
To do this, we first must specify the tolerances with the
setTolerance() method; then, we can check it with the
Resetting the Controller¶
It is sometimes desirable to clear the internal state (most importantly, the integral accumulator) of a
PIDController, as it may be no longer valid (e.g. when the
PIDController has been disabled and then re-enabled). This can be accomplished by calling the
Setting a Max Integrator Value¶
Integrators introduce instability and hysteresis into feedback loop systems. It is strongly recommended that teams avoid using integral gain unless absolutely no other solution will do - very often, problems that can be solved with an integrator can be better solved through use of a more-accurate feedforward.
A typical problem encountered when using integral feedback is excessive “wind-up” causing the system to wildly overshoot the setpoint. This can be alleviated in a number of ways - the WPILib
PIDController offers an integrator range limiter to help teams overcome this issue.
Enabling this setting with the
setIntegratorRange() method will prevent the total output contribution from the integral gain from exceeding the user-specified bounds.
Setting Continuous Input¶
If your mechanism is not capable of fully continuous rotational motion (e.g. a turret without a slip ring, whose wires twist as it rotates), do not enable continuous input unless you have implemented an additional safety feature to prevent the mechanism from moving past its limit!
The continuous input function does not automatically wrap your input values - be sure that your input values, when using this feature, are never outside of the specified range!
Some process variables (such as the angle of a turret) are measured on a circular scale, rather than a linear one - that is, each “end” of the process variable range corresponds to the same point in reality (e.g. 360 degrees and 0 degrees). In such a configuration, there are two possible values for any given error, corresponding to which way around the circle the error is measured. It is usually best to use the smaller of these errrors.
To configure a
PIDController to automatically do this, use the
Clamping Controller Output¶
Unlike the old
PIDController, the new controller does not offer any output clamping features, as the user is expected to use the loop output themselves. Output clamping can be easily achieved by composing the controller with WPI’s
clamp() function (or
std::clamp in c++):