PID Servomotor Control

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 its 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. 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 much as you need 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 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 your 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 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 motor's 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 of 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, whereas 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 whichever 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 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:

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, the code presented here is part of the Linux based PC robot project (https://www.linuxpcrobot.org), the purpose of which is to make a very inexpensive robot out of common and reused components wherever 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. Currently the Linux PC robot uses the digital to analog converter of a Velleman K8000 board, the Linux I²C interface driver, and a −5 to +5 dual H-bridge amplifier built some 15 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.

Once you have your motors fitted 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 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 the class is dedicated to setting up data and the set/get routines have been omitted. The only 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 versus 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.

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

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. The m_bias value is a static offset 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 "stop." Other systems may work between 0 and 255, with 128 being "stop." The bias is a way to 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.

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 descended 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
        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 PIDLoop. Let's examine it section by section.

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

This 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 via 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 cannot 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 detect an error in the system and exit the loop.

while (!ReadEncoders())
    m_stats.errors++;

These two lines read the encoder system.

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

This line records the elapsed time since 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.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 important line finds out how long this loop iteration took to run.

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

These two lines decide 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!

Conclusion

Obviously the code presented represents a small portion of a 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 https://www.linuxpcrobot.org.