PID Servomotor Control

By Mark L. Woodward, Independent Consultant

markw@linuxpcrobot.org

Introduction

In the process control field there is nothing more common than the PID, or the Proportional Integral Derivative, algorithm. It has been around for decades, and while there have been attempts to improve it by adding more control information, or replace it all together, it is still the bedrock from which all process control springs. Yet, as common and popular as it is, its functioning seems to be taken more as a “black box” by hobbyists who may basically understand it but who are a bit intimidated to delve too deeply into its operation. While it's behavior is founded in math that may be a bit complex for some beginners, it can be explained in conceptual terms that are easy to understand.

Closed Loop System

PID is a “closed loop” system, meaning that the results of the output drive system are known to the input control system. A closed loop can best be explained by the process of adjusting the volume of your radio or CD player. You adjust the volume based on what you hear. Your ears provide feedback to your brain which is controlling your hand which is in turn adjusting the volume. Your ears create a closed “feedback” loop allowing you to set the volume you like.

Closed loop systems are vitally important to process control because almost no control solution operates in a perfectly linear and predictable environment. There are always unexpected forces working for or against the process. For example: the oven in your home uses a closed loop system to control the cooking temperature. Most ovens use only a simple adjustable thermal switch (set to the desired temperature), it goes on when it is too cold, and off when it is too hot. This simplest of controls is able to react to the changes in temperature due to heat loss from normal radiation, temperature differential between the mass in the oven (say, a 20lb turkey) and the oven set point, and of course, the sudden and drastic loss of energy from an open oven door caused by an overly curious cook not content to use the oven window.

Imagine if your oven did not have the thermal switch. In engineering terms, It should be possible to calculate the amount of energy required to heat the oven to 350 degrees. You could also take in to account someone opening the oven door for 5 seconds, on average once an hour. Of course, there is also the outside room temperature that affects heat radiation of the oven and the possibility that you may open the door more or less frequently, and don't forget that a 20 pound turkey will absorb more energy than a two ounce english muffin with tomato sauce and cheese. When all the variables are accounted for, simply cooking your late night mini-pizza would become a major engineering feat.

Using the feedback loop thermal control system, (the thermal switch dial on your oven), you don't need to calculate all these variables. All of them, as well as the ones you didn't expect, can be monitored simply by measuring the oven temperature. (that was the purpose of the control in the first place!) You don't need to know whether it is colder in the kitchen or that there is a big turkey in the oven, since they both tend to absorb heat. The system need only respond to changes as they come, turn the oven on when it is too cold and turn the oven off when it is too hot, simple.

A closed loop system simplifies process control by changing the nature of the problem. Rather than indirectly factoring in all the possible variables and quantify their individual and cumulative affects on the output, you simply control the output. All the other things take care of themselves.

Proportional Control

Unlike your oven, controlling more demanding devices, like a motor or motorized vehicle, by simply turning it on and turning it off is probably a bad idea. For instance, a typical oven set at 350 degrees will drop to about 325 degrees before it turns on, then shoot up to about 375 before it turns off. This is a huge range of error. Imagine the cruise control system of an automobile set for 60 miles an hour speeding up to 75 miles an hour and dropping down to 45 before it speeds up to 75 again.

You don't need to switch a motor on and off. Using the car analogy again, you don't have to floor the gas pedal and then remove your foot, you can moderate the power based on the difference between actual speed and desired speed. Basically, you hold your foot down only as low as you need too to maintain the correct speed. This is “proportional control.” You add a proportional amount of power based on the relationship between actual speed and desired speed.

Proportional control works mostly on the notion of “error.” The error is how far or behind the device is from where it should be. A greater error requires a greater amount of force to overcome it. A smaller error requires a smaller amount of force. Theoretically, a proportional system in operation is always in an “error” position. If the error were zero, the output would also be zero. Again, like you car on cruise control, when you are going down hill, and you are at or above your speed, the gas pedal is almost all off.

PID Control

Your car doesn't need to stay exactly at 60 miles per hour, “cruise control” will be close enough give or take a couple miles per hour. What if, however, you need much tighter control?

The problem with a strictly proportional control of devices is that the error in speed is only a single measurement, a snapshot in time. A simple proportional system does not consider if the system is gaining or losing on its target or the rate at which it is gaining or losing. This is the “differential” and “integral” information in “PID,” and if ignored will cause the control system to respond poorly. Over compensating when it is catching up, and under compensating when it is losing ground.

The integral, or the 'I' in PID, is an accumulation of error, it compensates for previous behavior, it is sort of the ghost of error past. Each time the error is calculated, it is added to a number which represents the total accumulated error for the control process. As the this value increases or decreases from each error calculation, it is added to the proportional power output. Basically, even if a motor's current error is zero, (it's going the correct speed) the integrated error can still affect the motors power to make up for previous errors in overall motion, an important step if the system is working in conjunction with other systems, for instance the classic two wheel drive robot where if the two opposing wheels move different distances it turns.

The differential, or the 'D' in PID, is the rate at which the error is changing, it adjusts for anticipated behavior. If the current error is less than the previous error, the system is gaining on where it should be. If the current error is greater than the previous error, then the system is losing. The differential information can be used to modify the proportional control even further to prevent over-shoot or make it catch up more quickly. The differential value is added into the proportional output along with the integral value to augment the effects of the proportional or integral values.

Servomotor

After the discussion of how to control a motor, it makes sense to now discuss the motor itself. The type of motor we will be controlling is a “servomotor.”

Another motor you may be familiar with is something called a “stepper motor” they are very common in radio remote control applications. A “stepper motor” has a number electromagnetic “steps” or poles, spaced evenly around its axis. It is little more than a circle of electro magnets pulling the axle from one electro-magnet to the next like the needle of a compass. Control is merely energizing and de-energizing the electro-magnets in the right order and speed. While they excel in applications which require absolute precision in placement as the exact position of the motor can be set and held, they are ill suited for heavier duty tasks like a larger rolling robot. They consume a lot of energy in relation to power output.

In our application we will discuss the common permanent magnet DC servo motor. This is a simple device and it is often little more than an electric motor that reports movement with a rotational encoder. The encoder typically tracks a motor shaft's exact rotation forward or back and it is up to the process control system to translate this information in to something usable.

A servo motor is like a stepper in that the exact position of the motor can be known. The stepper can be placed by controlling the steps, a servo can be placed by counting the movement of the motor shaft. The difference is that the stepper can be held in position easily, were as a servo must have a control system that can manage the motor based on the feedback of the encoder, applying force to oppose movement.

Implementation

Now that all the concepts are out of the way, there needs to be a discussion about implementing theoretical algorithms in a real world application. The world is not a clean or ordered environment, similarly neither are most computer systems.

Regardless of the operating system and computer used on your robot project, unless it is a very tightly controlled “real-time” or embedded system, you will not be able to count on very precise time intervals. Most general purpose operating systems like Windows, Mac OS/X, Linux, and the various UNIX ® systems are horribly sloppy when it comes to timing. You'll have to code your algorithms to take this in to account. Fortunately many modern computers (like the Intel ® Pentium and later) systems can often maintain very precise timing information in hardware. This means that rather than code your algorithms based on precise process scheduling, you base them on precise time measurement.

The PID algorithm described previously doesn't mention timing at all, why is it important? Because many of the theoretical treatments of control avoid the practical problems of the implementation. By basically describing the relationships between the input and output, it leaves the implementor free to use which ever methodology they desire to implement the control system. In fact, a working PID system can be built with simple analog components and has virtually no timing constraints. This article, however, is about implementing a PID algorithm as a computer application.

Because we are implementing the control system with a computer, it (the computer) needs to be able to read how much the motor has moved and figure out how long it took to do it. If the computer reads that the motor shaft revolved 20 times, that information is meaningless unless we know how long it took. As an example, the trip odometer of your car may read 30 miles have been traveled, but if you don't know how long it took to drive that far, you could never estimate an average speed.

The implementation presented calculates PID along with some extra parameters not normally included in other PID descriptions. The calculation looks like this:

PO = (((E*E_GAIN) + (I*I_GAIN) + (D*D_GAIN)) * S) + BIAS

Variables:

PO: The calculated power output

E: The current positional error

PE: The previous error

D: The differential of the error

I: The integration of the error

S: The "scale" of the calculation based on elapsed time since last loop iteration

BIAS: The offset to "zero" for PO plus acceleration

Notes:

The variable 'PE' (previous error) is simply the error from the last iteration of the control loop (PE = E).

The variable 'I' is calculated by adding 'E' on each iteration of the control loop (I = I + E), this provides the integration of the error.

The variable 'D' is calculated by subtracting the previous error from the current error (D = E – PE), this provides the differentiation of the error.

The variable 'S' (scale) is used to adjust the output calculation for a variable or indeterminate sample period. It is calculated based on elapsed time since the last iteration of the control loop.

The variable 'BIAS' allows for two things, first, systems in which "stop" or zero output is not numerically zero, second, if you are accelerating or decelerating, You can tweak the output power up or down in anticipation of the change in speed for the next sample.

Hardware

If you are going to be controlling motors or a robot, you probably have some hardware in mind already, but for point of reference or if you don't already, the code presented here is part of the linux based PC robot project, (http://www.linuxpcrobot.org) the purpose of which is to make a very inexpensive robot out of common and reused components where ever possible. The price target for the robot is $500, and it is being designed as a general purpose robotics platform. While your project may be different, the code should take very little modification to fit your application.

The first device to consider is the motor encoder. The code presented uses a hacked PS/2 ball mouse. The X and Y axis wheels are mounted to the back of two motors. You may use any device you wish, as long as it can read the rotational movement of the motors. You will need to create an “Mencoder” class that presents your encoder to the code.

The next device is a motor drive system. This is a bit trickier. Currently the Linux PC robot uses the digital to analog converter of a Velleman K8000 board, the Linux I2C interface driver, and a -5 to +5 dual H-bridge amplifier I built some15 years ago (in keeping with the spirit of using what's available.) You will need some system that can take a drive number and drive a motor at a proportional power level. There are a number of devices available to do this. (Hint, check the ads on this site!)

Once you have your motors befitted with encoders and driven with a computer controlled power circuit, we can operate the motors using a PID algorithm.

Software

The first part to examine is the PIDControl class. It is mostly an object structure for containing the variables needed for a PID calculation.

class PIDControl

{

protected:

// Settable/readable parameters

double m_gain_dif; // Gain for differential

double m_gain_int; // Gain for integral

double m_gain_error; // Gain for error

double m_acceleration; // Acceleration

int m_bias; // Offset bias for algorithm

int m_pidMax; // Maximum output value

int m_pidMin; // Minimum output value

// Operational variables

double m_error; // The proportional error

double m_integral; // The integral value

double m_differential; // The differential value

public:

PIDControl(void);

virtual ~PIDControl();

virtual int CalcPID(double target, double actual, double scale=1);

virtual void SetDifGain(double gain);

virtual void SetIntGain(double gain);

virtual void SetErrGain(double gain);

virtual void SetBias(int bias);

virtual void SetAccel(double accel);

virtual void SetMin(int min);

virtual void SetMax(int max);

virtual double GetDifGain(void);

virtual double GetIntGain(void);

virtual double GetErrGain(void);

virtual int GetBias();

virtual double GetAccel();

virtual int GetMin();

virtual int GetMax();

virtual void Clear(void);

};

PIDControl::PIDControl()

{

m_gain_error = 1; // Gain for error

m_gain_int = 1; // Gain for integral

m_gain_dif = 1; // Gain for differential

m_bias = 0; // Offset bias for control

m_pidMax = INT_MAX; // Output clamp, max value

m_pidMin = INT_MIN; // Output clamp, min value

m_acceleration = 0; // Boost for acceleration

m_error = 0; // The proportional error

m_integral = 0; // The integral value

m_differential = 0; // The differential value

}

int PIDControl::CalcPID(double target, double actual, double scale)

{

// Calculate positional error

double error = target-actual;

// Calculate the differential between last error

m_differential = error - m_error;

// Update integral with new error

m_integral += error;

// Save the last last error

m_error = error;

// Sum the raw PID components

double rawpid=(

(error*m_gain_error)+(m_integral*m_gain_int) +

(m_differential*m_gain_dif))*scale;

// Adjust for acceleration and bias

int pid = m_bias+(int)round(rawpid+m_acceleration);

// Return clamped PID value

return (pid > m_pidMax) ? m_pidMax :

(pid < m_pidMin) ? m_pidMin : pid;

}

In the above code, most of class is dedicated to setting up data and the set/get routines have been omitted. The only the function that does anything interesting is “CalcPID.” Let's examine it line by line.

double error = target – actual;

This line calculates the error between the projected motion of the motor verses the actual motion.

m_differential = error – m_error;

This line takes the difference between this error and the previous error, this is the “differential” of the PID calculation.

m_integral += error;

This line adds the current error to the total error of motion. This is the “integral” component of the PID calculation.

Now, I know what you are thinking, just taking the difference between the current error and the last error is not a “differentiation,” and just adding up the error is not an “integration,” but remember this function will be run over time!

m_error = error;

Save the current error to be used for calculating the differential next time.

Now the big calculation:

double rawpid=((error*m_gain_error)+(m_integral*m_gain_int)+(m_differential*m_gain_dif))*scale;

This is basically the summation of the P, I, and D components multiplied by their gains and adjusted by the “scale.”

Ok, I bet you're confused! I never even mentioned scale, and while it is sort of bad form to introduce an important character in the middle of the plot, you'll have to hold your horses until we cover the PIDLoop function.

int pid = m_bias +(int)round(rawpid+m_acceleration);

This line takes the result of the PID calculation and adds it to a bias value as well as an acceleration value. These two values are pretty simple. The “m_bias” value is a static offset value used to adjust the PID software to external control systems. For instance, some motor amplifiers will accept a positive or negative number to drive the motor forward or backward with zero being “off” or “stop.” Other systems may work between 0 and 255, with 128 being “off” or “stop.” The bias is a way adjust for these differences.

The m_acceleration value is a way to have the algorithm anticipate a change in target value. Without it, the PID system assumes it is holding a constant speed. With this value you can drive the motors with a slightly higher or lower power value based on acceleration or deceleration so when the next time this routine is run with a higher target value,

return (pid > m_pidMax) ? m_pidMax : (pid < m_pidMin) ? m_pidMin : pid;

This last line returns the power output value. Its job is to ensure that the returned value does not exceed the minimum or maximum values the system can take.

The next section of code is a loop that repeatedly calls the CalcPID function. Its job is to manage the control loop and calculate the timing. It is part of the MotorControl class, which in turn is decended from the Servo class. Most of the set/get as well as the setup functions have, again, been omitted, leaving only the critical functions.

class Servo

{

protected:

PCONFTYPE m_prop_gain; // Proportional gain

PCONFTYPE m_diff_gain; // Differential gain

PCONFTYPE m_int_gain; // Integral gain

PCONFTYPE m_pid_min; // PID minimum value

PCONFTYPE m_pid_max; // PID maximum value

PCONFTYPE m_pid_bias; // PID bias

public:

Servo();

virtual ~Servo();

virtual Boolean ReadEncoders(void)=0;

virtual int GetEncoderValue(int motor)=0;

virtual int GetEncoderPosition(int motor)=0;

virtual void SetEncoderPosition(int motor, int value)=0;

virtual void StopMotors(void)=0;

virtual int SetPower(MOTORSTATS *motors)=0;

};

class MotorControl : public Servo

{

protected:

CONTROLSTATS m_stats; // Operation statistics

PIDControl m_pidctl[MAX_MOTORS]; // PID controllers

HistTimer m_timer; // Loop timer

HistTimer m_pid; // PID function timer

public:

MotorControl();

virtual ~MotorControl();

virtual void PIDLoop(PathPlanner *planner);

virtual int SetMotorCount(int nmotors);

virtual void SetPidErrGain(double gain);

virtual double GetPidErrGain(void);

virtual void SetPidIntGain(double gain);

virtual double GetPidIntGain(void);

virtual void SetPidDifGain(double gain);

virtual double GetPidDifGain(void);

virtual void SetPidBias(int bias);

virtual int GetPidBias(void);

virtual void SetPidMax(int max);

virtual int GetPidMax(void);

virtual void SetPidMin(int min);

virtual int GetPidMin(void);

virtual void ClearPid(void);

virtual int SetLoopPeriod(int usec);

virtual double ScaleGain(double gain);

};

void MotorControl::PIDLoop(PathPlanner *planner)

{

// Clear the stats structure

m_stats.minT=INT_MAX;

m_stats.maxT=INT_MIN;

m_stats.avgT=0;

// See if the callback loop is willing.

if(!planner->PIDStart(&m_stats))

return;

// Get the current time.

m_timer.Start();

for(m_stats.cycles=0; 1; m_stats.cycles++)

{

int i;

// Make sure no error

if(fErrorStop == TRUE)

{

StopMotors();

break;

}

// Get start to time how long this cycle takes.

m_pid.Start();

// Call encoder subsystem to read and save its

// encoder values.

while(!ReadEncoders())

m_stats.errors++;

// Calculate the exact elapsed time

double pidscale = m_stats.elapsed =

m_timer.GetElapsedTime(TRUE);

// Calculate the scale for fluctuations in timebase for

// PID consistency.

pidscale = pidscale/m_stats.pid_period;

// Track the min, max, and average times

m_stats.minT = MIN(m_stats.minT, m_stats.elapsed);

m_stats.maxT = MAX(m_stats.maxT, m_stats.elapsed);

m_stats.avgT += m_stats.elapsed;

// Call callback function.

// Here the planner can figure out what it wants to do

// this cycle, or quit.

if(!planner->PIDStep(&m_stats))

break;

for(i=0; i < m_stats.nmotors; i++)

{

// Save last power setting

m_stats.motors[i].lastpower =

m_stats.motors[i].power;

// Get the encoder value.

m_stats.motors[i].encoder = GetEncoderValue(i);

// Get the target movement based on elapsed time

m_stats.motors[i].target =

planner->GetTarget(i, m_stats.elapsed);

// Calculate the PID based on the target movement

m_stats.motors[i].power =

m_pidctl[i].CalcPID(m_stats.motors[i].target,

m_stats.motors[i].encoder, pidscale);

}

// Set all outputs at the same time, ioboard may be

// able to use DMA to be speedier.

SetPower(m_stats.motors);

// Measure how long the PID calculation took, and

// subtract that from the loop interval, that's

// what we'll wait

// If the PID time took longer than the loop interval,

// don't wait

int timePID = m_pid.GetElapsedTime(FALSE);

if(timePID < m_stats.pid_period)

planner->MicroSleep(m_stats.pid_period - timePID);

}

planner->PIDEnd(&m_stats);

}

As before, the only interesting function is the PIDLoop function. As before, lets examine it section by section.

if(!planner->PIDStart(&m_stats))

return;

The above set of code needs a little explanation. While not required to implement a PID algorithm, it is used to tailor this PID implementation to operate using a programmable path system using a PID callback system.

m_timer.Start();

The m_timer object is fundamental to the operation of the system. Since we are implementing the PID algorithm on a system that can not guarantee precise scheduling, we need to know precise timing.

for(m_stats.cycles=0; 1; m_stats.cycles++)

This line is a basic endless loop.

if(fErrorStop == TRUE)

{

StopMotors();

break;

}

The above lines look to detect an error in the system and exit the loop.

m_pid.Start();

This line starts a timer that times how long the loop takes.

while(!ReadEncoders())

m_stats.errors++;

The above two lines reads the encoder system.

double pidscale = m_stats.elapsed = m_timer.GetElapsedTime(TRUE);

This line records the elapsed time this section of code was last called.

pidscale = pidscale/m_stats.pid_period;

Remember back in the CalcPID function there was a variable named “scale?” Here is where it comes from. It is used to compensate for the normal variations in scheduling to be expected on general purpose operating systems.

m_stats.minT = MIN(m_stats.minT, m_stats.elapsed);

m_stats.maxT = MAX(m_stats.maxT, m_stats.elapsed);

m_stats.avgT += m_stats.elapsed;

The above three lines are for statistics generation, and can be ignored.

if(!planner->PIDStep(&m_stats))

break;

The above two lines are, again, used for the path planning system.

for(i=0; i < m_stats.nmotors; i++)

This system can handle more than one motor. This loop is used to handle each motor.

m_stats.motors[i].lastpower = m_stats.motors[i].power;

This line is for statistics gathering and can be ignored.

m_stats.motors[i].encoder = GetEncoderValue(i);

This line gets the encoder value previously read with “ReadEncoders().”

m_stats.motors[i].target = planner->GetTarget(i, m_stats.elapsed);

This line calls the path planner to get the next target.

m_stats.motors[i].power = m_pidctl[i].CalcPID(

m_stats.motors[i].target, m_stats.motors[i].encoder, pidscale);

This line calls the previously covered CalcPID function with the target, the encoder information, and the scale.

SetPower(m_stats.motors);

This line calls the robot system to update the motor amplifier values.

int timePID = m_pid.GetElapsedTime(FALSE);

This is an important line, it is used to find out how long this loop iteration took to run.

if(timePID < m_stats.pid_period)

planner->MicroSleep(m_stats.pid_period – timePID);

The above two lines decides how much the loop should sleep until the next run. If it took longer than the normal sleep period, it won't sleep. Otherwise, it subtracts how long it took to run from the normal sleep period. It's all just guess work anyway as the operating system decides when to return, that's why we pass the “pidscale” value to the CalcPID function.

planner->PIDEnd(&m_stats);

Lastly, call the path planner system and inform it that we are done.

Tuning the Algorithm

The last issue to deal with is getting the right values for proportional, integral, and differential gain, without which the motors will almost certainly perform badly. It will be sluggish or oscillating and trying to destroy itself. There are a number of methods for tuning a PID system, but the classic method is usually good enough.

The classic tuning method is fairly simple, set all gains to zero, increase the proportional gain until the system just starts to oscillate. Increase the integral gain until the oscillation stops. Lastly increase the differential gain so it reaches the speed setpoint within an acceptable period of time.

For the robot builders reading this, you'll find that tuning your robot will be a bit more difficult. If your robot has any mass, it will not tune well sitting on a stand. You have to tune it as it is moving because the motors will behave differently under load. Tuning a mobile robot requires a bit of patience: wheel irregularities, debris on the floor or rough flooring will provide a drastically varying environment. Also, you'll probably never get it 100% right in the whole speed range, you may need to try tuning the PID system for different speeds and accelerations and set the gain variables when required.

Warning – Read This Before Operating!

The PID algorithm has the ability to compensate for errors in motion. The source of these errors is not known. Hardware failure, poor tuning parameters, a sudden change in “integral” gain can cause the system to run away.

If you follow the classic tuning method, you may want to clear or restart the system before you start tuning the integral gain. If you ran it with no integral gain for any period of time, there is probably an amount of accumulated error in motion that will cause your system to run away at full speed, trying to catch up on lost movement. If you are tuning a mobile robot, you can expect it to run away as fast as it will go!

It is ALWAYS a good practice to have an emergency stop system incorporated into any control system. Software, hardware, operators, and engineers are not perfect, there is always a risk of “run away” condition in any system.

Conclusion

Obviously the code presented represents a small portion of much larger whole. What was presented does cover the important aspects of motion control using a PC running Linux (or modified for your operating system). You are invited to download all the code at http://www.linuxpcrobot.org.