PID Control Joe Ross Team 330

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

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

What is Control

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

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”

Simple Methods of Control

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

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

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

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

PID Control

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

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

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

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

PID Tuning

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?

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

FRC Programming Example

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

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. }

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

Velocity PID

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

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 0.0002. 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

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

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

Other Considerations

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)

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

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

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)

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

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

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: http://www.embedded.com/design/embedded/4211211/PID-without-a-PhD Wpilib.screenstepslive.com: http://wpilib.screenstepslive.com/s/3120/m/7912/l/79828-operating-the-robot-with-feedback-from-sensors-pid-control LabVIEW Documentation (PID Control (PID and Fuzzy Logic Toolkit)) http://www.chiefdelphi.com forums and whitepapers

Backup

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;

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; }

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;

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);

LabVIEW PID Implementation