Lesson: Exercise: Using a PID Controller

With the test robot used to develop this course, there is a problem with the previous examples of turning under gyro or IMU control. When turning at a constant power setting and setting the power to zero when the target angle is reached, depending on motor configuration, gear ratio, robot weight and the turn power, the robot will most likely not stop quick enough to be on the desired angle. This is called overshoot. On our test robot, a 90 degree turn would end up being 110-120 degrees. Fixing this can be tricky to do manually but there is an automated way to better control the turn.

To do this we will use a software routine called a PID controller. PID stands for Proportional, Integral, Derivative. The idea behind a PID controller is to take the desired state value (90 degrees in our turn example), compare that to the actual (feedback) state value (the current gyro or IMU angle) and apply factors to the difference (called the error) that produce a proportional output value. So in our example we start out turning away from zero towards 90 degrees at the full power we specify. As the turn progresses, the angle of turn is fed to the PID controller routine which measures error and produces a value (the turn power) at or near the starting (full) power. As the turn gets closer to 90 degrees, the PID routine starts to return smaller and smaller values thus reducing the power being applied and slowing the rate of turn. In theory this reduction in power and slowing rate of turn will eliminate the overshoot. The PID controller can also apply a tolerance margin that indicates when the actual value is within some percentage of the target to further control robot motors.

A similar example is to use a PID controller to compute the power correction needed to make the robot drive in a straight line. Here the target is zero (gyro not deviating from direction we are traveling) and any change in the measured angle will result in a correction value calculated by the PID controller.

A PID controller can take a lot of tuning to get the desired result but once you get a feel for how they work you can tune them fairly quickly and by using all three of the PID factors you can get quite fine control over motors. 

There are many resources and discussions of PID online. Here, here and here are some resources to start with to investigate PID further. The FIRST forums on programming have extensive discussions of PID in robot applications.

Here is the source for a class called PIDController that will perform the PID calculations. Create a new class called PIDController and paste that code into it. Because PIDController is in the same package as the examples below, Java will be able to locate it when you reference it in the examples. PIDController is a library or utility class, a class that does nothing by itself but is used by other classes.

The example below takes the previous DriveAvoidImu example and uses two PIDController instances to manage straight driving and the 90 degree turn on obstacle contact. 

An obvious question is how did we arrive at .003 for the P value on the turn PID controller. In the case of moving from a non-zero error to zero error, we took the maximum error value, 90, and the power level we want applied at max error, .30 (30%). We divided the power by the max error ( .30 / 90 = .003 ) to determine P.

Often with just a P value alone, the robot may stall out before completing the turn because the PID controller reduces the power below the level which will move the robot. To fix this, we add some I (integral) value. The I value will compensate for P not reaching the setpoint and start adding power until the robot completes the turn. A good starting I value is P / 100. You can adjust I to get the turn completed accurately in a timely fashion. Note that these values are optimal for a 90 degree turn with 30% power. They will not work as well for other angles or power levels. You would have to compute new values for other angle/power combinations. Here is the above example modified to compute the P and I values for any angle/power combination input to the rotate() function.

In the case of driving straight, the target and error are the same at the start so the error is zero. So we have to determine how much correction we want to apply for how much error. Experimentation showed that .05 (5%) correction power for 1 degree of error worked well, correcting the error without overshooting too much (wandering): .05  / 1 = .05 for P.

As always, you can do these calculations to determine a starting P and I values and then adjust then to tune actual robot behavior.

In most simple cases only a P value is needed. I may be needed if you can't find a P value that reaches the setpoint with out overshooting. A discussion of using the D value is beyond the scope of this lesson.