PID Motor Control with an Arduino

pid_firstbot

PID motor control with an Arduino can be accomplished using simple firmware.  In this example we use our Firstbot Arduino-Compatible controller to implement a PID based position controller using analog feedback and a potentiometer for control.  This is similar in operation to a hobby servo, but the potentiometer provides the control signal instead of a pulse from a receiver (and of course you are using a motor, not an RC servo).

FIRSTBOT_PID

PID control is fairly common means of controlling a system using a well defined algorithm.  You can learn more about the algorithm at Wikipedia (see PID Controller). They are used in industrial control systems of all types.  In this case I’m using it to control the position of a motor.  I have a potentiometer that outputs a 0-5VDC control voltage.  Attached to my motor shaft is an analog encoder that also outputs a value of 0-5VDC over a single rotation of the motor (US Digital MA3 encoder).   The idea is that when I turn the pot the motor follows the turn and stops where the pot stops.

P, I, and D stand for proportional, integral, and derivative.  Generally speaking the proportional (P) part of the control algorithm provides most of the “push” to get things moving.  The integral (I) term is used to act on small errors and force gradual changes over time.  The derivative (D) part of the equation acts to damp oscillations or abrupt changes in the control signal.  The D term is usually works to oppose abrupt changes caused by the P term.

Here’s the code for the equation I used.  The gain constants are set in the code, but could be made programmable through some other interface.

void CalculatePID(void)
{
// Set constants here
  PTerm = 2000;
  ITerm = 25;
  DTerm = 0;
  Divider = 10;

// Calculate the PID  
  Accumulator += Error[0];  // accumulator is sum of errors
  PID = Error[0]*PTerm;     // start with proportional gain
  PID += ITerm*Accumulator; // add integral gain and error accumulation
  PID += DTerm*(Error[0]-Error[9]); // differential gain comes next

Here are the major components of the PID detailed.

Error Signal-  At the heart of PID control is a need to measure an error signal.  In this case it is the desired position (voltage from the pot) minus the actual position (voltage from the encoder).  The error value is signed.

void GetError(void)
{
  byte i = 0;

  // read analogs
  word ActualPosition = analogRead(ActPos);  

  word DesiredPosition = analogRead(DesPos);

  // shift error values
  for(i=9;i>0;i--)
    Error[i] = Error[i-1];

  // load new error into top array spot  
  Error[0] = (long)DesiredPosition-(long)ActualPosition;

}

PTerm –The error signal is multiplied the P term and this provides most of the “umph” behind the motor’s movement.  A negative error signal causes the P term create a negative motor movement.  Likewise, if my error signal is 0 then the P term has no impact on the motor.

  PID = Error[0]*PTerm;     // start with proportional gain

ITerm – The I term is generally much smaller than the P term.  There is also an accumulator associated with the I term.  The accumulator sums up error signals over time.  Eventually even small errors build up to be large number, when that happens a small I term will  cause to move the motor.

  Accumulator += Error[0];  // accumulator is sum of errors
  PID += ITerm*Accumulator; // add integral gain and error accumulation

In some systems it is important to prevent “windup” of the accumulator(also called saturation).  Windup occurs when the small errors build so high that when movement finally occurs it takes a long time for the accumulator to reduce to an insignificant amount.  I didn’t add any windup protection here, but it can often be addressed by limiting the size of the accumulator.

Example:

if (Accumulator > 40000)

Accumulator = 40000;

if (Accumulator < -40000)

Accumulator = -40000;

DTerm – While I have the D term in this equation it is not used for this motor control application (see DTerm = 0 in the code).  The D term is multiplied by the change in the error signal (error – last error).  Sometimes it is useful to store your error measurements in an array and use as your last error something a little further back in time.  For fast changing systems, like a motor controller, the derivative portion of the PID has little impact unless you make it very large, or compare error signals with adequate time between their sampling.  In this code the derivative error is the latest error signal minus the 10th previous error.

  PID += DTerm*(Error[0]-Error[9]); // differential gain comes next

Divider – When you put the PID together you get a pretty big value.  This value needs to be scaled to a value that matched the pulse-width modulation range for the controller.  The Divider does that.  You’ll notice that the division of the PID is accomplished by right-rotates as opposed to division.  This is just a faster way of accomplishing the same thing.  And the faster your PID loop runs the more responsive it can be to commanded changes.

 PID = PID>>Divider; // scale PID down with divider

Converting the PID to PWM- Once your PID is calculated you need to change it to a motor drive signal.  The sign of the PID output determines the direction the motor should be driven and the divider previously discussed should get you in the right neighborhood for a final number.  Now you need to make sure the PID register contains a value that’s neither too large or too small.

  if(PID>=127)
    PID = 127;
  if(PID<=-126)
    PID = -126;

//PWM output should be between 1 and 254 so we add to the PID    
  PWMOutput = PID + 127;

The FIRSTBOT accepts a range of 1 to 254 (1 = full reverse, 127 = stopped, 254 = full forward).  So we want our PID to be in the area of –126 to +127, and we’ll add 127 to it to get our 1-254 range.

Tuning the PID- Trail and error works.  There are a number of methods of tuning PID algorithms, you can research those online.  For something like a generic motor position controller using analog signals you can start by adjusting your proportional settings until you get rough control.  If your proportional term is too high the movement will be sharp and choppy.  If too low, it will be slow.  This is also when you dial in the divider value to make sure your PID output falls within your PWM requirements.

Then increase your integral term until the final desired position is reached.  The integral term is usually much smaller than the proportional term.  An integral term that is too high will cause oscillations of the motor.  Too low and it has no impact at all.  If your motor tends to twitch you may need to add some kind of windup detection as discussed above.

The settings I used for this design got me to within +/-10 ADC counts within a couple of seconds.  That is right at about 1% accuracy or 3.5 degrees for a single rotation.

The code has a lot of commented out print statements that can be commented in during certain tests to see what those variables look like.  Those can be useful when tuning the PID.

PID Loop Time:  You can see from this code that it is very simple.  The serial communication that sends data to the FIRSTBOT’s other motor controller IC (a PIC16F1829) occurs every 10mS.  Not super fast, but it certainly works for many applications.

Here’s all of the code together.

/*
  Firstbot PID code:  Implements a PID controller using
  analog inputs for actual and desired positions.

 The circuit: 
 * RX is digital pin 2 (connect to TX of other device)
 * TX is digital pin 3 (connect to RX of other device)

 */
#include <SoftwareSerial.h>

// define some constants
int ActPos = A0;    // select the input pin for feedback signal
int DesPos = A1;    // select the input pin for control signal

byte PWMOutput;
long Error[10];
long Accumulator;
long PID;
int PTerm;
int ITerm;
int DTerm;
byte Divider;

/* 
The FIRSTBOT has a PIC16F1829 controller that controls the 
two MC33926 H-bridges on the board.  A oftware serial interface
is used to control that part.
*/
SoftwareSerial mySerial(2, 3); // Receive data on 2, send data on 3
byte SerialTXBuffer[5];
byte SerialRXBuffer[5];

void setup()  
{

 // Open serial communications and wait for port to open:
  Serial.begin(9600);
  mySerial.begin(9600);
}

/* GetError():
Read the analog values, shift the Error array down 
one spot, and load the new error value into the
top of array.
*/
void GetError(void)
{
  byte i = 0;
  // read analogs
  word ActualPosition = analogRead(ActPos);  
// comment out to speed up PID loop
//  Serial.print("ActPos= ");
//  Serial.println(ActualPosition,DEC);

  word DesiredPosition = analogRead(DesPos);
// comment out to speed up PID loop
//  Serial.print("DesPos= ");
//  Serial.println(DesiredPosition,DEC);

  // shift error values
  for(i=9;i>0;i--)
    Error[i] = Error[i-1];
  // load new error into top array spot  
  Error[0] = (long)DesiredPosition-(long)ActualPosition;
// comment out to speed up PID loop
//  Serial.print("Error= ");
//  Serial.println(Error[0],DEC);

}

/* CalculatePID():
Error[0] is used for latest error, Error[9] with the DTERM
*/
void CalculatePID(void)
{
// Set constants here
  PTerm = 2000;
  ITerm = 25;
  DTerm = 0;
  Divider = 10;

// Calculate the PID  
  PID = Error[0]*PTerm;     // start with proportional gain
  Accumulator += Error[0];  // accumulator is sum of errors
  PID += ITerm*Accumulator; // add integral gain and error accumulation
  PID += DTerm*(Error[0]-Error[9]); // differential gain comes next
  PID = PID>>Divider; // scale PID down with divider

// comment out to speed up PID loop  
//Serial.print("PID= ");
//  Serial.println(PID,DEC);

// limit the PID to the resolution we have for the PWM variable

  if(PID>=127)
    PID = 127;
  if(PID<=-126)
    PID = -126;

//PWM output should be between 1 and 254 so we add to the PID    
  PWMOutput = PID + 127;

// comment out to speed up PID loop
//  Serial.print("PWMOutput= ");
//  Serial.println(PWMOutput,DEC);

}

/* WriteRegister():
Writes a single byte to the PIC16F1829, 
"Value" to the register pointed at by "Index".  
Returns the response 
*/
byte WriteRegister(byte Index, byte Value)
{
byte i = 0;
byte checksum = 0;
byte ack = 0;

SerialTXBuffer[0] = 210;
SerialTXBuffer[1] = 1;
SerialTXBuffer[2] = 3;
SerialTXBuffer[3] = Index;
SerialTXBuffer[4] = Value;

for (i=0;i<6;i++)
  {
  if (i!=5)
    {
    mySerial.write(SerialTXBuffer[i]);
    checksum += SerialTXBuffer[i];    
    }
  else
    mySerial.write(checksum);     
  }
  delay(5);

  if (mySerial.available())
    ack = mySerial.read();

  return ack;
} 

void loop() // run over and over
{
     GetError();       // Get position error
     CalculatePID();   // Calculate the PID output from the error
     WriteRegister(9,PWMOutput);  // Set motor speed
}

Comments

  1. Student says:

    anyone plz can explain the role of WriteRegister() function and who it can control the PWM ??

    • The WriteRegister() function is used to write data to the Firstbot’s PIC16F1829 microcontroller registers. The Firstbot has two microcontrollers on board. One operates as an Arduino Uno. The other uses the same communication protocol as our BM011. WriteRegister() matches the communication protocol that is defined in the Firstbot datasheet.

      The PIC16F1829 controls 2 H-bridges and can output 4 RC style pulses. It can also read RC style pulses. In the case of this application the PID algorithm is being handled by the Arduino Uno and WriteRegister() is used to control the PWM output of one of the H-bridges connected to the PIC16F1829.

      Take a look at the Firstbot datasheet for more clarification (should be a link in the blog post).

      Lon

  2. Pretty! This was a really wonderful article. Thanks for providing this info.

  3. Hi mate,

    we use your pid control for our antenna tracker project (see website). A member of the community where we develop the tracker found an error in our code which is part of the code we took from you:

    long Error[10];

    for(i=0;i size 10
    but in the for loop you go to Error[9+1] which is out of range of the array…

    Hope this helps you as well.

    Best regards

    Samuel

  4. can i use arduino motor shield instead of firstbot for this project?

    • The concepts related to a PID algorithm are transferable to all control systems. Whether or not the specific way I implemented this is transferrable to the Arduino motor shield kind of depends on how that shield works, and I have never used it.

  5. i like to read this code for controlling the servo by PWM but i need more clarify or to solve my confusing for me come from the following:
    1- the calculating error of the desired and measured signal what the dimension of it (degrees, mdegrees, ….)
    2- the controller gains it is in order of thousands like P=2000, …
    3- the output of PID divided by 10 divider how to calculate the divider
    3- the out of PID to get the PWM output added 127 is it in msec
    4- the range of servo from 1 to 254 is it in ms also

    thanks

    • Hi Alaa,

      Here are some responses…

      1: The error signal initially would have the units of what is being measured, in this case volts. But the error signal is converted to PWM by the PID process, so I guess in this case I guess it becomes direction and duty cycle of motor speed (which I guess is pretty much still volts).

      2-3: You could use floating point math to have the gains look more like 1.0 or 0.0055. In the end I was trying to achieve a few things. First, the proportional gain will probably be the largest of the PID, and it needs to act on small error signals. Second, the I term needed to have some effect, but not so much as to cause chatter or windup. Third, the input error signal needed to scale roughly to the output I needed. Depending on your system the I and D terms may not even be used.

      Whatever error signals in a system requires the PID output to be scaled to be pretty close to the desired drive signal. The gain settings are, in this case, based on the type of variable used in the system (int and long), and some trial and error. The divider I used is a divide-by-two (right rotate). Our error signal is 10 bits and our gain is about 9 bits, so I used those magnitudes as an estimate for the divider’s number of rotates.

      3-4 You can take a look at the FirstBot datasheet on PWM frequency. The Firstbot accepts an 8-bit speed and direction value with 127 being stopped. So these values are not mS. They translate to duty cycle and direction. 127 being 0% on, 254 being 100% on, and 1 being -100% on (reverse full speed).

      Lon

  6. Shachar says:

    Hey there,
    I’m afraid you have an off-by-one mistake on this for loop:
    for(i=0;i<10;i++)
    Error[i+1] = Error[i];

    it should be
    for(i=0;i<9;i++)
    Error[i+1] = Error[i];

    Thank god I'm dating a programmer! She helped me find it! the data overrun was causing all sorts of erratic behavior on my code and I'm just not smart enough for these things…

  7. Hi Lon,

    very good presentation on the PID code on an Arduino!

    Just one question (I wonder why nobody else caught it before): Shouldn’t the index i in the loop rotating the former error values count down instead of up? When counting up IMHO you copy the value of the most recent error into all the other cells.

    The way the code is provided above a two-element buffer would have had the same effect…

    Kind regards,
    Tony

    • Hi Tony,

      Thanks, you’re correct. It may be that nobody caught it because nobody actually reads the code (including me apparently) :).

      A two buffer array, or two variables would also work. I think I wrote the code using the larger buffer to allow for some time span between the error signals that was larger than the PID update rate. But as you pointed out the D term would always be 0 with the loop going in the direction presented. I’ll correct it.

      Lon

  8. How to calculate the constants?

    • PID constants are different for every system. Often they are determined experimentally. You can Google PID tuning to get an idea of the algorithms / process that some people suggest.

      I usually set the integral and differential term to zero and make adjustments to the proportional term until control is pretty good. Then I increase the integral term until the final position is reached within an appropriate amount of time. I usually fiddle with the differential term last, if I use it at all.

  9. Hammad Ali says:

    Thw PWMOutput gives only two values, either ‘1’ or ‘254’. Shouldn’t it be giving all the possible values between 1 and 254??..

Speak Your Mind

*