PROGRAMMING IV
Advanced Motor Control
What are some problems with
controlling motors like this?
Open Loop Control
Closed Loop Control
Open vs Closed Loop
Simpler to set up Complicated setup
Requires little testing Requires tuning
Gets the job done for simple Almost required for complex
systems systems
Requires you to know what Can independently generate
power to set the system to the input
PID Controller
Proportional, Integral, Derivative -> PID
PID Controller ≠ Motor Controller
Mathematical construct that helps a system
reach some state
PID Equation
Setpoint or r(t): The desired state of the system with
respect to time
Output or y(t): The measured state of the system with
respect to time
Error or e(t): The difference between the setpoint and
the output.
Control Effort or u(t): The output added by the
controller with respect to time
How does PID work?
Uses 3 constants: kP, kI, kD (known as gains)
Performs calculations on the error and adds
the result to the input, thereby reducing the
error over time.
Proportional Term
u(t) = kP * e(t)
Makes sure the output reaches the setpoint
Does most of the heavy lifting
Integral Term
u(t) = kI * ∫ e(t)dt
kI multiplies the sum of all error
Eliminates “steady state error”
Usually not needed
Derivative Term
u(t) = kD * d/dt e(t)
kD multiplies the rate of change of e(t)
Tries to make the rate of change of the output
match the rate of change of the setpoint
Helps slow the system down when it
approaches the setpoint
What happens when kP is too high?
A large kP causes overshoot and oscillation around the
setpoint
Can be fixed by lowering kP or increasing kD
What happens when kI is too high?
A large kI can cause overshoot
The Integrator term can result in “wind-up”
If the Integrator isn‘t reset, it can add to much to the
output
What happens when kD is too high?
A large kD can result in unpredicatable behaviour and
aggressively dampen the system
How are gains calculated
PID gains can be determined experimentally
through trial and error, until the system behaves
as expected
WPILib has a tool called SysID that can calculate
gains for you by performing a test routine
Setting up PID
How do you track the current state of a system?
RECALL: Encoders measure rotational movement.
Brushless motors (like Neos) have built-in
encoders)
PID Classes
WPILib PIDController
REV SparkPIDController
CRTE Phoenix Velocity and Position
ControlMode
Adding PID
Create a PIDController object with some gains
Use .calculate to get u(t). .calculate requires y(t)
and r(t) as parameters.
Adding PID
Since y(t) and r(t) are both in units of rotations,
u(t) will be in units of rotations.
.set() vs .setVoltage()
.set() uses a percent value
.setVoltage() uses a voltage value
.setVoltage prevents instability when the battery
voltage drops
How to get RPM for a Neos
Create a RelativeEncoder object by calling
.getEncoder on a SparkMAX.
.getVelocity returns the measured speed.
How to get RPM for a Neos
Create a RelativeEncoder object by calling
.getEncoder on a SparkMAX.
.getVelocity returns the measured speed.
Tips
.calculate is not a set-it and forget-it method, so it
must be called continuously. You may need to
update your Commands to use runEnd instead of
startEnd.
