1. PID Control
Joe Ross
Team 330

2. Agenda What is Control Simple Methods of Control PID Control Explained
PID Tuning
Java example
LabVIEW example
Velocity PID Control
Other Considerations

3. Intro Joe Ross Team 330 (Beach Bots) electrical / programming mentor
17 years FRC experience
3 years as a student with 330
14 years as a mentor
Control System Advisor at regionals
Programmed FRC Robots In PBASIC, C, LabVIEW, Java
Electrical Engineer & Systems Engineer

4. What is Control

5. The Problem
How do you make the robot do “something” accurately and precisely?

6. The Solution First – Be able to measure “something”
Need a Sensor
Drive the robot forward 10 feet
Need to measure the distance the robot is driving
Move the arm to 45 degrees
Need to measure the angle of the arm
Shoot a ball 15 feet
Need to measure the distance of the ball
What if this isn’t possible?
Measure something that is proportional to the desired measurement
Second – Use the data from the sensor to do “something”
First – Be able to measure “something”
Need a Sensor
Drive the robot forward 10 feet
Need to measure the distance the robot is driving
Move the arm to 45 degrees
Need to measure the angle of the arm
Shoot a ball 15 feet
Need to measure the distance of the ball
What if this isn’t possible?
Measure something that is proportional to the desired measurement
Second – Use the data from the sensor to do “something”

7. Simple Methods of Control

8. Terminology Process Variable (Sensor Reading) Setpoint Error
Expressed in terms of a sensor reading, or physical units
Potentiometer reads 3 volts
Encoder reads 200 counts
Robot has traveled 10 feet (Encoder reads 200 counts)
Physical units are easier, but sensor reading must be converted to physical units before used in PID
Setpoint
The desired position. Uses same units as Process Variable
Error
How far away is the sensor reading from the setpoint. Error = setpoint – Sensor Reading
Step Change
Instantaneous change in setpoint

9. What if there isn’t a sensor?
Do something based on time
Drive at half speed for 2 seconds
Adjust speed and time to drive 10 feet
What can go wrong?
Battery Voltage variation
Friction Variation
Wheel Slippage
Obstacles
If you can move +/- 1 foot per move, What is the range (+/-) after 5 moves?
+/- 5 feet
Do something based on time
Drive at half speed for 2 seconds
Adjust speed and time to drive 10 feet
What can go wrong?
Battery Voltage variation
Friction Variation
Wheel Slippage
Obstacles
If you can move +/- 1 foot per move, What is the range (+/-) after 5 moves?
+/- 5 feet

10. Stop at Sensor Reading Move until sensor reads setpoint, then stop
Advantages
Simple
Disadvantages
Robot will always be at or (more likely) past setpoint
Amount past setpoint will vary for same reasons as timed control

11. Bang-Bang Control
Move forward when robot is before setpoint, Move backward when robot is after setpoint
Advantages
Simple
Disadvantages
Continuously cycle around setpoint
Add Hysteresis to reduce cycling
Used in heating / cooling systems
Turn on Air Conditioner at 78, Turn off Air Conditioner at 75

12. PID Control

13. Proportional Control Need to determine Kp
Output is proportional to distance from setpoint
Start fast, and slow down as you get closer
Error = Setpoint – SensorReading
Output = Kp*Error
Need to determine Kp
Much too small: don’t move
Too small: move slowly, don’t reach setpoint
Too large: oscillate
Disadvantage:
Need to tune (select best value for Kp)
Doesn’t reach setpoint

14. Integral Control Output is proportional to integral (sum) of error
The longer time and farther distance away from setpoint, the faster you go
Output = Ki*(Error + TotalError)
TotalError = TotalError + Error
Used in Combination with Proportional Control
This solves the problem of not reaching the setpoint
Disadvantages
Two values to tune
Potentially unstable

15. Derivative Control
Output is proportional to derivative (difference from previous) of error
The faster the it is reaching the target, the slower it should go
Output = Kd*(Error – Previous Error)
Used in combination with P or PI control
Disadvantages:
2 or 3 values to tune
Won’t reach setpoint without I

16. PID Control
Proportional, Integral and Derivative Control are combined to make PID Control
Error = Setpoint – Sensor Reading
Total Error = Total Error + Error
Error Difference = Error – Previous Error
Output = Kp*Error + (Ki*Total Error) +
(Kd*Error Difference)
Previous Error = Error

17. PID Tuning

18. Measures of Effectiveness
Rise Time
How long it takes to get to setpoint
Overshoot
How far above setpoint it goes
Settling time
How long to stop oscillating
Steady State Error
How close to setpoint
Stability
Does it go crazy?

19. PID Tuning Set Kp, Ki and Kd to 0
Increase Kp until gets close to setpoint
Stop if behavior is good enough
Increase Ki until reaches setpoint quickly
Increase Kd until oscillation is reduced
Effects of increasing a parameter independently
Parameter
Rise time
Overshoot
Settling time
Steady-state error
Stability P
Decrease
Increase
Small change
Degrade I
Eliminate D
Minor change
No effect in theory
Improve if small

20. FRC Programming Example

21. PIDController class (Java / C++)
WPILib in Java and C++ contain a PIDController class
3 things needed to use the PIDController class
Sensor that implements PIDSource
Accelerometer, AnalogChannel, Counter, Encoder, GearTooth, Gyro, HiTechnicCompass, and Ultrasonic classes
Motor Controller that implements PIDOutput
CANJaguar, Jaguar, Talon, Victor
PIDController Object
Creates a thread for each PIDController object that runs in the background and runs the PID calculation
SmartDashboard element for changing PID parameters

22. Java Example
public class ArmDown extends CommandBase { public ArmDown() { requires(arm); } protected void initialize() { arm.armPID.setSetpoint(2.0); arm.armPID.setAbsoluteTolerance(0.1); arm.armPID.enable(); protected void execute() { protected boolean isFinished() { return arm.armPID.onTarget(); protected void end() { protected void interrupted() {
public class Arm extends Subsystem {
private SpeedController armMotor = new Jaguar(5);
private AnalogChannel armPot = new AnalogChannel(3);
public PIDController armPID = new PIDController(1,0,0,armPot,armMotor); }
public PIDController armPID = new PIDController(1,0,0,armPot,armMotor);
PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output)           Allocate a PID object with the given constants for P, I, D, using a 50ms period.
protected void initialize() {
arm.armPID.setSetpoint(2.0);
setSetpoint(double setpoint)           Set the setpoint for the PIDController
arm.armPID.setAbsoluteTolerance(0.1);
setAbsoluteTolerance(double absvalue)           Set the absolute error which is considered tolerable for use with OnTarget.
arm.armPID.enable();
enable()           Begin running the PIDController }
protected boolean isFinished() {
return arm.armPID.onTarget();
onTarget()           Return true if the error is within the percentage of the total input range, determined by setTolerance. }

23. LabVIEW PID Inputs Outputs
Output Range – [-1, 1]
Setpoint
Process Variable – Sensor
PID gains – [Kp, Ki, Kd]
Dt (s) – Leave unconnected
Outputs
Output – Connect to Speed Controller Set Output
Put in periodic tasks, 20 ms loop. Recommend setting setpoints with global variables

24. Velocity PID

25. Position vs Velocity PID
Everything to this point has been about Position PID
Most literature covers Position PID
Position PID sets output to 0 when reaches setpoint
Want to stop when reaches position
Velocity PID
If output is set to 0 at setpoint, the item being controlled will stop
Friction or other external forces
Need to make sure output doesn’t go to 0 at setpoint

26. Feed Forward Feed forward adds a constant to the output
Keeps output from going to 0.
Output = (Kp*Error) + (Ki*Total Error) +
(Kd*Error Difference) + Kf
Want to run shooter at 3000 RPM. Manually control motor and find that at 60% output, shooter is 3000 RPM, set KF to 0.6.
Works for a single setpoint
Base Feed Forward on setpoint
(Kd*Error Difference) + (Kf*Setpoint)
Want to run shooter between 2000 RPM and 4000 RPM. At 2000 RPM, output is 40%. At 4000 RPM output is 80%. Set KF to
PID then just has to adjust differences from feed forward term
Fast to react to setpoint changes
Feed Forward is available in both Java/C++ and LabVIEW

27. Integrating Output Use PID to adjust previous PID output
Output = (Kp*Error) + (Ki*Total Error) +
(Kd*Error Difference) + Previous Output
Slower to react to step changes, since output depends on previous output, which was based on previous setpoint
Tuned the same way as Position PID

28. Position PID Library
What if you’re stuck using a PID Library that doesn’t support Feed Forward or Integrating Output? (Jaguar)
Treat it like an Integrating Output
Set Derivative to 0
Tune Integral like proportional
Tune proportional like Derivative
May not behave as well, since you are using a PD controller

29. Other Considerations

30. PID Forms Different people write the PID equation differently
Kp + Ki + Kd
Kp * (1 + Ki + Kd) = Kp + Ki*Kp + Kd*Kp
Kp + 1/Ki + 1/Kd
Kp * (1 + 1/Ki + 1/Kd) = Kp + Kp/Ki + Kp/Kd
All will behave the same, but different PID constants must be chosen
If Ki and Kd are in denominator, must make smaller to have larger impact when tuning
LabVIEW uses Kp * (1 + 1/Ki + 1/Kd)

31. 330’s PID uses Control the position of an arm
Potentiometer mounted to arm
Useful in both Autonomous and Teleop.
Control the speed of a shooter wheel
Encoder or Hall effect magnet sensor to measure speed.
Bang Bang control may work if wheel has enough mass
Much simpler to implement and no tuning required
Turn an angle in autonomous
Gyro mounted to chassis
Left = Gyro PID
Right = -Gyro PID

32. 330’s PID uses Drive a distance in autonomous
Encoder in transmissions. 1 PID controller, average left and right sensor values for Process Variable. Set same output to both sides
Mechanical differences in left and right
2 PID controllers, Left drive and Right drive. Encoder in transmission.
Will correct for mechanical differences between left and right, but usually only at end
3 PID controllers, Left drive, Right drive and Gyro.
Left = Left PID + Gyro PID
Right = Right PID – Gyro PID
Wheels may slip at startup, due to PID output typically going to max on a step change
Use Camera data as a setpoint
Distance to target, Angle to target
Or use Arcade Drive

33. PID Improvements Integral Windup Timing
Limit the amount of integral error that can accumulate
Improves stability
If process is slow to respond
If process is constrained
Timing
All provided pseudo code assumes that PID algorithm is called at a constant rate
Need to adjust integral and derivative terms if timing varies from call to call
Time = Current Time – Previous Time
Output = (Kp*Error) + (Ki*Total Error)*Time +
(Kd*Error Difference)/Time
Previous Time = Current Time
LabVIEW PID does this, Java and C++ PIDController class do not, but is in a separate thread which should help minimize timing variation (I have not measured)

34. PID Improvements Make updates easy
Able to change PID constants without recompiling or redownloading program
Java/C++ SmartDashboard, Preferences class
LabVIEW: INI file
Able to use the same setpoint in multiple places (Telop/Autonomous)
Able to change setpoints quickly

35. PID Prerequisites A good sensor, suitably mounted
Sensor must have resolution several times better then setpoint
Can’t measure arm +/- 1 degree and control it to +/- 0.5 degrees
Sensor can’t slip or drift
Sensor can’t be too noisy
Good mechanical structure
Tuning can be a violent process if you make a mistake. Needs to be able to handle full speed changes
PID will make a good robot better. It won’t make a bad robot good.
Time to tune
Tuning PID is an art and takes time. Do not try to implement PID the hour before bagging
Practice robot and real robot may need separate PID constants

36. PID Resources
Intro to control theory is typically a Senior level College Electrical Engineering class
Taught at a much more theoretical level, doesn’t look anything like this
Many more control methods are taught in Graduate school
PID without a PHD:
Wpilib.screenstepslive.com:
LabVIEW Documentation (PID Control (PID and Fuzzy Logic Toolkit))
forums and whitepapers

37. Backup

38. Java PIDController Implementation Calculate (1 of 4)
private void calculate() { boolean enabled; PIDSource pidInput; synchronized (this) { if (m_pidInput == null) { return; } if (m_pidOutput == null) { enabled = m_enabled; // take snapshot of these values... pidInput = m_pidInput; if (enabled) { double input = pidInput.pidGet(); double result; PIDOutput pidOutput = null;

39. Java PIDController Implementation Calculate (2 of 4)
synchronized (this) { m_error = m_setpoint - input; if (m_continuous) { if (Math.abs(m_error) > (m_maximumInput - m_minimumInput) / 2) { if (m_error > 0) { m_error = m_error - m_maximumInput + m_minimumInput; } else { m_error = m_error + m_maximumInput - m_minimumInput; }

40. Java PIDController Implementation Calculate (3 of 4)
if (m_I != 0) { double potentialIGain = (m_totalError + m_error) * m_I; if (potentialIGain < m_maximumOutput) if (potentialIGain > m_minimumOutput) { m_totalError += m_error; } else { m_totalError = m_minimumOutput / m_I; else m_totalError = m_maximumOutput / m_I;

41. Java PIDController Implementation Calculate (4 of 4)
m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F;
m_prevError = m_error;
if (m_result > m_maximumOutput) {
m_result = m_maximumOutput;
} else if (m_result < m_minimumOutput) {
m_result = m_minimumOutput; }
pidOutput = m_pidOutput;
result = m_result;
pidOutput.pidWrite(result);

42. LabVIEW PID Implementation