Read your chosen feedback value and subtract it from
your desired terminal value. This is called the error term. You
can get this any way you want — via wheel encoder, motor
back EMF readings, or anything else you wish to use. It will
be: error = (desired speed – current speed).
Calculate your P term. It will be: kP (error term) =
Vproportional.
Calculate your I term. It will be: kI SUM(all past error
terms) = Vintegral.
Calculate your D term. It will be: kD (current error –
last error) = Vderivative.
Calculate your Correction term. It will be: Vproportional
+ Vintegral + Vderivative. Note that Vderivative will most
likely be a negative term.
Apply your correction term to your PWM setting; this
will set the voltage to the motor.
A PID algorithm will have terms that are used to multiply
times the error term. These terms will supply the gain of the
term. Think of them as amplifiers for the error term in the
PID algorithm. Typically, they are denoted with a lower case
k. In order of our definition above, they are kP, kI, and kD.
You’ve probably heard the term “tuning a PID loop.” This is
the process of tweaking the three terms above to give your
PID algorithm the response you desire.
That isn’t so scary, is it? There are more sophisticated
ways to calculate these values and come up with proper
corrections that will work with your PWM generation, but
careful selection of your PID gains will allow your loop (called
a loop because you do it over and over again) to return sane
values. Your friendly neighborhood SERVO Magazine website
has a C program written in CCS PCM for the PIC16F73
microcontroller that allows you to play with your PID
algorithm to see what gain values cause what responses.
You can download this program at www.servomagazine.
com under Mr. Roboto as picpid.zip.
I’ll describe the various functions of this demonstration
code first. Picpid will allow you to control the speed and
direction of a DC motor through a serial interface at 115200
baud. You are free to choose your favorite processor, but I
recommend that you use one that has a hardware PWM, as
well as a hardware USART, so that your PID algorithm can
run as fast as it can in the background. This program does
not use interrupts to take the error term; it simply operates
at a sample rate of every 5 ms.
To do the best job with a PID algorithm, you should
have the sample rate repeat at a constant rate so your error
terms will be proportional to both the time they are taken
and the error value that you get. Picpid is simply an example
program that will allow you to play with PID values to see
what effects they have on your hardware.
Picpid uses the ADC hardware to measure the CEMF (or
back EMF) of the DC motor to determine how fast it is
spinning. This isn’t incredibly accurate, but it requires no
other sensors to be used beyond that required to get the
voltage from the motor wires. Listing 1 shows the GetError()
function which reads that back EMF. Feel free to modify this
to get your quadrature readings instead. (Listing 1, 2, and 3
are available on the website, as well.) Here, in this function,
16 SERVO 11.2008
you should scale your readings to allow them to match
up with your PWM settings. I’m using a 10 bit PWM in
the PIC hardware, so I scaled my readings up by two to get
better response.
signed long getError()
/* Find the difference between where we want to be and
where we are. */
{
signed long error;
unsigned int16 ma,mb;
setup_ccp1(CCP_OFF);
delay_us(500);
set_adc_channel(0);
delay_us( 20);
ma = read_adc();
ma <<= 1;
//Turn off PWM
//wait for steady state
//get fwd side voltage
// “Amplify” the signal, making it
// more relevant
set_adc_channel(1);
delay_us( 20);
mb = read_adc();
mb <<= 1;
//get reverse side voltage
currPos = (signed long)((signed long)ma - (signed long)mb);
//printf(“ma= %lu mb= %lu\n\r”,ma,mb);
setup_ccp1(CCP_PWM_PLUS_ 3);
error = calcPos - currPos;
return error;
}
Note that new settings are only sent to the program
when you press the L key. This allows you to set several
attributes before the motor has to respond to them.
Picpid is a very simple PID implementation, but it works.
There are bugs in it, such as interesting motor behavior at
boundary conditions that I didn’t test and correct for. Since I
chose to use 16-bit signed calculations, the resolution isn’t as
great as it could be, but again, it works. Experiment with the
settings to see what happens. You may wish to change the
program to make it implement a positional servo mechanism
instead of one controlling motor speed. It can be done!
I will leave this as an exercise for the student. You too
can implement PID in your motors quite easily. However, I
caution you — a PID algorithm of sufficient sample speed
will take over your microcontroller if it is a low speed
device. I recommend that you implement PID on a dedicated
microcontroller and talk to it via a serial connection like this
one or SPI or even I2C so that your main computer can set
its “mind” to higher things that you want your robot to
be doing.
I hope that you’ve learned something this month.
As usual, I can be reached for questions, comments, and
criticisms at roboto@servomagazine.com and I’ll be happy
to work on it! Until next time, keep on building those
robots! SV
