1. The Basics of PID Control in Robots
Engineering II – LBHS

2. Process Control: Control Systems
Many everyday tasks require a control system, whether manual or automatic. For example, maintaining a constant speed while driving a car requires a control system. You may compare your current speed to the desired speed, and manually adjust the acceleration. Alternatively, you may set a cruise control system to accomplish the same task automatically using a network of sensors and throttle controls.  Humans are good at controlling processes, but sometimes human control is costly, dangerous or impractical, such as when a process requires extreme precision and repeatability. In these cases an automated process control system is advantageous.

3. Process Control: Automated Control
Every process includes controllable variables, such as the flow rate of a liquid, the temperature of the environment, or the power given to a motor. These variables all influence the outcome of the process.  Automated process control requires an intelligent link to take the place of human control between the variables and the output. Sensors must monitor the variables, and the system responds to the sensor data. For example, the climate control system monitors the current temperature. When the temperature falls to a certain level, the system responds by increasing the heat output until the desired temperature is reached. 

4. Process Control: Example of Automated Control
Consider a factory in which a single pump moves a chemical to several reaction tanks. The rate at which the chemical enters each tank must be carefully controlled.  If the valve to one of the the tanks closes, the rate at which the liquid enters the remaining tanks increases.  An automated control system can immediately decrease the power supplied to the pump, thereby reducing the rate at which the chemical is added to the remaining tanks. 

5. Process Control: On-Off
The simplest process control system would be one where there is constant power applied to a robot, and then the power is turned off as soon as a target goal is reached.
Think of having a simple joystick program to control a two-motor robot where simply pushing one of the buttons would turn on both motors on at full power, and releasing the button would turn off the motors.
There are several disadvantages to this approach:
Power can only be applied at a constant level.
'On' could be too fast or too slow depending on the situation.
Driving this way is not impossible, but using a joystick control would be much easier.

6. Mobile Robotics is About Navigation
In mobile robotics (and fixed industrial robotics for that matter) there is always some place or value to which you are trying to “get.”
Go until you get to that value, then stop.
Even with the best sensors, however, we need smart code and smart algorithms so we don’t over-shoot the goal.

7. Process Control: Step Control
Say we want to control the speed of our robot.
Using an encoder, you could write a program that would incrementally add more power when the robot was moving too slowly, or less power when the robot was moving too quickly.
The program needs to check the new speed that results from the last power change to determine if another power change is necessary.
This would create the step-like response shown in the picture.

8. Process Control: Step Control
At each step: Have I reached my target speed? No. No. No. No. No.
On a steeper incline, the program that worked in the previous example might not. The motor response to the power increases might be so slow that the robot would stall before it reached its desired speed.
This is due to the time the program spends waiting to check for the new speed of the robot.
However, if the program didn't wait at least some time, the PWM values would reach their maximum before any physical response had a chance to manifest itself. It would over-adjust and exceed the target speed.

9. Process Control: Proportional Control
What is needed is a program that will apply more or less power to the motors as necessary.
In fact, this is exactly what happens when you drive your robot with tank or arcade drive. The more you push on the joysticks, the faster the robot drives.
This method of control is commonly referred to as proportional control.

10. Understanding Proportional Control: P Control vs On-Off System
Rather than simply turning a motor on and off with respect to a target speed, a proportional control algorithm constantly adjusts. Such an algorithm could read the robot's current speed and compare to its target speed.
The smoother response achieved by proportional control reduces wear and tear on the robot by removing sudden changes typical of an on-off control system. The proportional control system also smooths away the extreme highs and lows.

11. Understanding Proportional Control: The P Equation
The equation for proportional control is typically expressed as:
where m is the output, kp is the proportional constant (also called the gain), and E is the Error.
In this case, output (m) is the adjustment to motor speed, Error (E) is the difference between the robot's current sensor reading and the desired or target sensor reading, and kp is the proportional constant that converts the Error to a reasonable output.

12. Understanding Proportional Control: Understanding Error
Proportional control is a function of the Error, which is the difference between the current value and the target value.
For example, we would like to program a Vex robot to stay 15 inches from a wall using an ultrasonic sensor. If the robot is currently 24 inches away from the wall, then the Error in the system would be 9 inches. The proportional control would translate the positive Error of 9 into a motor value that drives the robot forward.
9 inches

13. Understanding Proportional Control: Understanding Error
Proportional control automatically compensates for direction. If the robot were 12 inches from the wall, the Error would be -3. Since the output in this case is a negative number, the robot would travel in reverse (away from the wall).
kp has as much effect on the output as the Error does. Determining the correct kp constant is important. Sometimes is a process of using trial and error to determine kp.

14. Understanding Proportional Control: Relating Error to Output
The P loop corrects the system by changing one or more controllable variables. Changes to the output depend on the type of application. Sensor feedback from a thermometer might produce an output that changes the power supplied to a heater. Sensor feedback from an ultrasonic rangefinder (such as on a moving Vex robot) might produce output that changes power supplied to a motor.
If a robot is driving straight toward a wall, the Error must be applied in such a way that both motors act equally. If the motors didn't act with the same speed, the robot would not travel in a straight line.

15. Understanding Proportional Control: Understanding Error
While the constant kp might change, Error is simply the difference between the current sensor value and the targeted sensor value.
In the case of trying to achieve a target distance from an obstacle using a sonar sensor, it is the difference between the desired distance and reading from the sensor.

16. Behavior of Proportional Control: Visualizing Proportional Control
Proportional control is aptly named. Since proportional control is a form of linear feedback, the magnitude of the response is simply the magnitude of the Error multiplied by a scaling constant (kp).
When plotted, the graph is a straight line.

17. Behavior of Proportional Control: Oscillation Concerns
At low Error values, the motors do not receive enough power to move the robot. To compensate for this, kp would become so large that the robot would never stop oscillating. Proportional control would always be overcompensating, with the robot always driving back and forth past its target.
The problem with robots and other moving systems is that momentum influences the response time. The robot cannot stop immediately when the P algorithm turns off the motors, as the robot's momentum keeps it moving longer.

18. Behavior of Proportional Control: Disadvantages
Momentum is what keeps the system oscillating in a manner similar to that shown in the picture. The more aggressive the response (resulting from a larger kp constant), the more extreme the oscillations will be.
On the other hand, a lower kp value has difficulty compensating for small Errors, and the robot will often settle down at the wrong location. This trade-off is the major disadvantage of a P loop.
Based on the previous example, it is easy to see why determining the correct kp constant is very important. If kp is too high, the system will destabilize. If kp is too low, the system will be unresponsive.
However, finding the correct value for kp can be challenging.

19. Tuning P: Effect of kp
For small gains, such as the orange curve in the picture, the robot reaches the position without overshooting, but does so very slowly. This system is referred to as over-damped.
With higher gains, just as the purple and green curves, the response time is increased, but the robot overshoots. These systems are under-damped. In the end, the system doesn't settle down any faster than with the lower gain.
The red curve depicts the behavior you are striving for. This system is critically damped, because it approaches the target location as quickly as possible without overshooting.

20. Tuning P: What is Response Time?
When the previous example referred to an increased response time, what did you envision 'response time' to mean? If you assumed response time meant the time it takes for the robot to finish reacting to changing conditions, you were correct.
There are differing opinions as to exactly how response time is measured, because a robot will 'finish' reacting at different times, depending on the application. For a robot with an ultrasonic sensor, represented by the graph in the picture, you could say the response time is the time that elapses until the robot reaches its target for the first time (even if it overshoots).

21. Tuning P: How To Estimate kp
When you first begin programming you will need to enter a kp value before you run the program for the first time. While the kp need not be correct, it should be a good 'ballpark' estimate. If your estimate is reasonable, you should recognize whether your system is under-damped, over-damped, or critically damped. You can then begin fine tuning the program by making minor adjustments to kp.
Look at the equations to the right. Try estimating kp in your head by envisioning reasonable output and Error values and solving the proportional control algorithm in reverse.
For a moving ultrasonic robot, possible motor outputs range from 0 to 255 inches (or 0 to 648 cm). Within that range, all speeds are fair game. Ask yourself how fast you think the robot should be moving when it is a given distance from its target. You would then choose a desired output value and solve for kp.
The values found using this method are not perfect. After fine- tuning, your final kp could be much higher or much lower than you expected. However, this method provides you with a starting point.

22. Tuning P: Low Errors and High Errors
For low Error values, you must remember that Vex motors have a deadband of roughly +/- 13 (more depending on load). This means there will be a range of output values where no robot movement will result. A higher kp will compensate for this, reducing the deadband by inflating the output.
The second consideration is the maximum output value generated by the proportional control.
The solution to the overflow dilemma is to allow the output to exceed its maximum and minimum value and limit it artificially before the value reaches the motor. When a reasonable output from a controller cannot be produced, the phenomenon is known as saturation. The following rules apply:
In ROBOTC, if the output is greater than 127, it is decreased to 127 (or your maximum target PWM).
Your P loop might output a value of 425 to the motors (the robot would want to go really fast!), but a conditional statement would limit any value greater than 127 to be 127 instead.

23. Tuning P: Saturation
For any Error values beyond the saturation point, the output to the motors will not change. A conditional statement in the program would limit the output to the maximum allowable value. Essentially, all Error values producing motor outputs above maximum appear the same.
These Error values are said to be outside the proportional band. Only Error values within the proportional band produce an output proportional to the Error.

24. To Review…. Proportional (P) Control is
A measure of how far you are away from your goal.
Error is computed by simply comparing a current value/reading (usually a sensor reading in robotics) to the target value
The larger the P value, the larger the motor output (in the case of mobile robotics) should be, since you have farther to go (whether a positive or negative distance).
You must fine-tune the kp value through trial and error.
It is part art (or at least it may seem like it!), part science, and occasionally, part luck!

25. Process Control: Derivative Control

26. Defining Derivative Control: What is Derivative Control?
Derivative control predicts future needs by calculating the change in Error over time. Think of derivative control as adding emergency brakes to your proportional control. As your robot experiences sudden changes in Error, accompanied by rapid acceleration (provided by the P loop), the derivative control counteracts the rapid acceleration, smoothing out the motion.
In derivative control, the magnitude of the Error, or how large the Error actually is, is not important. Only the relative change between Errors matters.

27. Defining Derivative Control: Derivative's Effect on P
Another way of looking at D is as a damping effect on the Proportional control. Unlike Proportional control, derivative control cannot exist on its own, since it is only a response to changing events. If the P control in a system oscillated 3 times before settling down, adding D might reduce the oscillations to twice or even a single time. Increasing D reduces overshooting, but it slightly increases the response time.
While adding D to a P loop may increase the response time for a given kp, it allows for stronger P loops (larger kp) to be utilized. In some applications, kp can be twice as large when D is added to control the system.

28. Defining Derivative Control: Derivative and Slope
In the picture, note the three points on the graph with a line tangent to the graph, shown in black. The slope of the tangent line at any point, which is also the slope of the curve, is the derivative. When P is increasing or decreasing quickly, the slope is steep. A steep slope corresponds to a large derivative.

29. Defining Derivative Control: Derivative and Slope
The slope and derivative can be zero. P settles down right before the robot changes direction where, for a brief moment, P does not change. The tangent line is horizontal with a slope of zero, and therefore the derivative is zero.

30. Calculating the Derivative: Eyes vs Circuits
The time period over which you evaluate the change in Error is critical to the successful use of derivative control. To the human eye, the Error appears to be constantly changing, but to a microprocessor, the sensor data may actually be changing infrequently.
In the image to the right, an ultrasonic sensor is mounted on a Vex robot traveling toward a wall 3 feet away. How many unique readings do you think the robot sees? How many total sensor readings do you think this robot is taking?

31. Calculating the Derivative: Faster than the Eye
In answer to the previous question, the robot only sees 37 unique readings (0 through -36), because the ultrasonic sensor only takes whole number readings and has traveled only 36 inches.
The robot may have taken hundreds or even thousands of sensor readings during this time, however! The only things restricting the sampling rate of the sensor, if the programmer has not interfered, are the processing speed of the hardware and the physical specifications of the sensor itself.
For example, even if the Vex robot has a rather slow loop time (20 ms) it will take 150 readings during a 3 second journey. Since the distance from the wall changed only 37 times, the vast majority of the readings taken were duplicates. In fact, the sensor would have shown no change almost 75% of the time. 25% of the time, the reading will have changed by an inch.

32. Calculating the Derivative: Sampling Interval
The graph in the picture represents data coming from the ultrasonic sensor as the robot moves away from a wall. Notice that once we 'zoom in', the data appears to change very slowly. The curve at the bottom, shown in red, represents the derivative. The red plot shows the change in distance if it were evaluated between every point. Notice that 75% of the time, the derivative is zero because there is no change in sensor readings. When the derivative does come into play, it is only very briefly because of the short sampling interval.

33. Calculating the Derivative: Sampling Interval (continued)
You can stabilize the effect of D by simply increasing the time interval between sampling points.
In the picture, the curve in yellow represents D if the derivative is determined using every fifth data point rather than consecutive data points (red curve).
Notice that D equals zero less often. More importantly, the magnitude of D is consistently higher over a greater length of time. As a result, changes in P response with respect to D will be smoother.

34. Calculating the Derivative: Sampling Interval (continued)
Increasing the sampling interval even further to every 10 data points, as shown in the picture, amplifies the smoothing effect even more. The purple line represents D using every 10th data point. Notice that the magnitude of D is much more substantial.
The derivative is also becoming less responsive as the sampling interval is increased. If the sampling interval is increased too far, the Error could change dramatically without a response from D. The goal is to choose a sampling interval that is smooth but doesn't allow for intervals long enough to miss important events.

35. Timers and D
For a Vex robot traveling at full speed with a front-mounted ultrasonic sensor, the smallest interval you would want between sensor readings is about 50 ms (we’ll use 100 ms in our program for starters).
You can use ROBOTC timer to adjust the time interval between readings.
The timer functions like a stopwatch, with a scale in milliseconds. You can start, stop and set each of the four (4) available timers, and each of the timers will operate independently of the rest.

36. Timers and D: Why We Need Timers
The simplest answer is that using the…
wait1Msec(time);
wait10Msec(time);
…commands will interfere with the sampling rate because they will not allow other code to be executed simultaneously with the wait command.
The timer differs from the wait command in that it can run in the background while other code is executed.
A timer is a good choice for programs with infinite 'while' loops that cannot possibly wait and still perform effectively.
ROBOTC Timer Syntax

37. D Control In ROBOTC: How Does This Help Me?
You are probably wondering how exactly a timer will help you calculate the derivative. You need a timer to slow the rate at which the derivative is calculated, without harming the proportional control algorithm you previously wrote.
A sample PD control loop is shown in the picture. To tune this loop, you would adjust the time interval over which the derivative is calculated, as well as kd. As with the tuning of P, this is not an exact science, and often involves repetitious trial and error.

38. Tuning D: Adding D to the Algorithm
Because D acts against the motion like a brake, it is subtracted from the proportional equation, rather than added to it.
The equation for PD control is then:
…where kd is the derivative constant, Eold is the previously sampled Error and E is the current Error.
In our PD program we’ll store the value of (Eold – E) in a variable called “derivative.”
Taking the difference between the Errors as is done here is similar to finding the slope between two points. Since the time interval between Error readings is constant (set using the timer command), time is not needed in the equation.

39. Tuning D: kd and Time
Two variables are responsible for derivative behavior; kd and the time interval between readings. Because the two variables are independent of one another, they must be tuned individually.
The best way to approach this is to choose a reasonable kd value, so that you can concentrate on choosing a correct time interval. Once the time interval is correct, you can return to kd for fine tuning.

40. Tuning D: Increasing kp
Because derivative control acts against the motion of the system, your robot will be slower and less responsive once the derivative control is added. For this reason, you should increase kp up to the level where constant oscillation occurs.
When properly tuned, the addition of derivative control will return the system to a reasonable level.

41. To Review…. Derivative (D) Control is
How quickly you are moving towards your goal by comparing the rate of change of the error measurements
Unlike P, when D is higher you want to “pull back” your motor output. This keeps you from heading towards you target too quickly. If P is still large (i.e. you are far from your target) then it will “overpower” the D.
In the example of a moving robot using a sonar sensor, this would be the speed we are traveling towards the target distance.

42. Integral Control: Limitations of Just PD
PD control suffers a serious shortcoming: the deadband in the motors prevents the robot from consistently ending exactly on target. Stopping at an exact position is generally beyond the scope of PD control.
The problem can be demonstrated by placing the robot on a hill. Using the same control algorithm the robot on the hill falls far short of the target. The power supplied to the motors at low Error values is not enough to propel the robot up the slope - it can only hold its position.
With PD control, the robot operating on a hill will never reach the target. Adding integral control could help the robot reach this target.

43. Integral Control: How Integral Control Works
In contrast to derivative control which predicts future needs, integral control responds to cumulative Error. The longer it takes to reach the target, the more Error accumulates, thus the more influential the I control becomes in the system.
If the robot is on a flat surface, and immediately snaps to the right spot, I control will have had practically no effect, because there will be no accumulation of Error.
However in the example of a robot on a hill, the robot slows but does not gain ground on the target because it is opposing gravity. Integral control would have an ever increasing effect because the Error would accumulate.

44. Integral Control: Area Under the Curve
An integral is an important concept in mathematics and calculus. The integral is a method of determining the area under the P curve (shaded in yellow). The integral is often referred to as 'the area under the curve'.
This is the most important aspect to understand when applying integrals in programming, and is illustrated in the following graphs.

45. Integral Control: Positive and Negative I
You are very familiar with the P curve and with D, the derivative of P at any point. Over the next few pages, the P curve will represent Error occurring as a robot with a gyro is spun left or right from its target position of 0 degrees. Because the target is at 0, the Error at any point on the curve is just the magnitude at that point (the distance from the X-axis).
The yellow region below the curve and above the X-axis is positive area, where as the region above the curve, and below the X-axis is negative. When calculating the integral at any point, sum all the positive and negative Errors that have come before. Remember that they cancel one another out.

46. Integral Control: Differences between D and I
In the picture, a close up view of the P curve shows the derivative slightly different at every point on the curve. Derivatives can change dramatically in a short time as the curve transitions from positive (uphill) to negative (downhill). The derivative at each point is independent of any previous or future derivatives.
The integral is different. Draw a line perpendicular to the X-axis from any point on the curve. The length of this line is the point's distance from the Y-axis. Think of each line as a rectangle. The integral equals the sum of the areas of all the rectangles. In this example, the integral is the cumulative Error that occurs as the robot attempts to get to its target position.

47. Calculating and Tuning the Integral: I vs PD
The curve in the picture represents the position of a robot approaching a target on a hill. Two possible outcomes are shown.
The dashed line represents the position of the robot under P or PD control. After settling down, the position of the robot will not change, because the Errors are too small to overcome gravity and motor deadband.
The solid line is the robot under the effects of I. As the Error grows, gradually more power is sent to the motors, and the robot moves towards the target.

48. Calculating and Tuning the Integral: Equation for I
Adding Integral Control to the Proportional Algorithm yields…
…where iError is the sum of all previous Errors. In easyC, you would sum the Error using the statement iError = iError + Error. Each time the loop executes, the current Error is added to the running total.

49. Calculating and Tuning the Integral: Integral Windup
The integral component of PID is traditionally the most unstable of the three. The best solution for dealing with this instability often depends on the application.
One challenge of integral control is integral windup, which results in loss of control over the system.
In the example in the picture, the robot becomes stuck, preventing it from driving forward. When this happens, the total Error 'iError' continues to increase until it overwhelms both P and D. Even if the robot extricates itself from the obstacle, it will find its behavior has been destabilized by its own I control.

50. Calculating and Tuning the Integral: Correcting Integral Windup
There is room for creativity in harnessing Integral Control, yet no single solution is always perfect. The previous example could be improved by limiting iError to a maximum value that will not dominate P and D.
This improvement has been implemented in the animation in the picture, but there is still something wrong. Do you know what it is?

51. Calculating and Tuning the Integral: Correcting Integral Windup (continued)
When the robot arrives at the correct location and the Error is zero, you would expect it to stop, but it doesn't. Instead it keeps driving forward, because iError is still at its maximum value (no negative Errors have been introduced to reduce it). The robot will continue to drive forward until P is strong enough to move it in reverse, or until the accumulation of negative Error has made iError less than zero. This oscillatory behavior will continue forever if not corrected.

52. Calculating and Tuning the Integral: Resetting I
To fix this behavior, reset the integral every time the Error is zero. While this doesn't completely eliminate the overshoot caused by I, it does significantly reduce it.

53. Calculating and Tuning the Integral: Methods for Tuning I
In many PID loops used in other applications, the Error is summed in every iteration of the loop, resulting in an extremely large I. To compensate for this, ki is usually an extremely small decimal, sometimes so small it is denoted using scientific notation.
If you are comfortable working with decimals, this traditional method of computing I may be the easiest solution. In this case, tuning I is a straightforward process in which you must select a proper ki constant, develop a system for resetting I when the Error is zero, and set a maximum value for I.
If you are not familiar with the use of floats and other decimal saving storage types, the best approach to tackling integral control is to use timers to reduce the frequency at which the Error is sampled and accumulates.

54. Calculating and Tuning the Integral: Methods for Tuning I
To tune a PID use the following steps:
Set all gains to zero.
Increase the P gain until the response to a disturbance is steady oscillation.
Increase the D gain until the the oscillations go away (i.e. it's critically damped).
Repeat steps 2 and 3 until increasing the D gain does not stop the oscillations.
Set P and D to the last stable values.
Increase the I gain until it brings you to the setpoint with the number of oscillations desired (normally zero but a quicker response can be had if you don't mind a couple oscillations of overshoot)

55. To Review…. Integral (I) Control is
The integral is how long you have been away from your target.
The idea is that the longer the robot has not made it to the target, the more power should be applied to get it there.
The most difficult part to incorporate.
PD control alone can be very effective.

56. PID Control: Brief Introduction Video

57. PID Control: Simple Examples of PID Control Video