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

    • Copy paste error…it should be
      for(i=0; i < 10; i++)
      Error[i+1] = Error[i];

      • Jagmeet Kumar says:

        I think
        for(i=0; i < 10; i++)
        Error[i+1] = Error[i];
        this is wrong.
        it will just copy Error[0] in all the array.
        Suppose Error[0] = 10,
        Now,
        Error[1]=Error[0]; // Error[1] value is now 10
        Error[2]=Error[1]; // Error[2] value is now 10
        Error[3]=Error[2]; // Error[3] value is now 10
        Error[4]=Error[3]; // Error[4] value is now 10
        Error[5]=Error[4]; // Error[5] value is now 10
        Error[6]=Error[5]; // Error[6] value is now 10
        Error[7]=Error[6]; // Error[7] value is now 10
        Error[8]=Error[7]; // Error[8] value is now 10
        Error[9]=Error[8]; // Error[9] value is now 10

        if i am wrong please corrent me.

        Regards,
        Jagmeet Kumar

        • A few people have pointed out errors in the D term loop (quite a while ago). I thought I changed the code in the post to sort down and not up. I couldn’t find the code you posted in the blog…

          I think for(i=0; i < 10; i++) Error[i+1] = Error[i]; this is wrong.…

          I see this on my page…
          // shift error values
          for(i=9;i>0;i–)
          Error[i] = Error[i-1];

          Somebody also pointed out that you don’t need an array of errors in the D term implementation. You can just have two variables Previous_Error and New_Error. The D part of the PID is calculated by…

          PID += DTerm*(New_Error-Previous_Error)

          I have found that in some motor control applications the difference in error terms between two PID updates is pretty small. At times it helps to track errors over a longer time frame to apply damping to step effects from sudden position changes.

          Lon

    • Samuel,

      Thanks for the update/correction.

      Lon

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

  10. Christopher Lowden says:

    Hello
    I am looking to do exactly what you describe above but i am total newbie. In the thread below, I am looking for a way to have a motor with a DIGITAL ENCODER follow an analog potentiometer over 270 degrees using an Arduino Leonardo and Cytron motor controller.
    http://forum.arduino.cc/index.php?topic=460220.0
    Do you think that your code could be adapted for my config?
    Thank you

    • Christopher,

      You’ll need to get the digital encoder signals into the Arduino. We of use the LS7083 to do this (an LSI part sold by US Digital in the US). This can also be done with logic chips. I usually design with Microchip’s PIC controllers where the encoder A and B channels can be fed into external clock hardware. I’m not sure which Arduino style has these features so you have to look around for examples.

      Once you have the encoder channel up and down count in the code your position becomes “up count – down count”. This then becomes the actual position in the PID code. The pot analog conversion becomes your desired position. The Error signal = (actual position – desired position).

      There are a few important aspects of this.

      1. You cannot poll the up and down counts, as they will change verify fast. However, if you have a low count encoder you might be able to use edge triggered interrupts to collect the changes.

      2. The encoder resolution and potentiometer resolution may need to be scaled to match each other. For example if you use a 10-bit analog converter, and your pot moves over 270 degrees, then you have 768 (1024*0.75) different values the pot can be at. If your encoder is 200 counts per revolution and your gearing is 1:1 then your encoder only has 150 counts over 270 degrees. If your gearing is 10:1 then your encoder has 1500. counts. You may want to multiply the analog value by encoder range / analog range so the two measurements are scaled to match each other.

      3. Regardless of the resolution of your encoder you are still limited by the resolution of your analog measurement. For example, if you had a 2000 count per revolution(CPR) encoder and a 10:1 gear ratio from encoder to motor output shaft, a single rotation of the motor shaft would give you 10*2000 possible encoder positions. But if the pot analog conversion is 10-bits (1024 steps), and that sets your desired position, then your system can only have 1024 possible positions to be in. Furthermore, analog signals are susceptible to noise from motor movements. You might have +/- 4 bits of noise in the signal you read from the pot. Your actual possible positions may end up being closer to 256. That gives you position accuracy of 360/256 degrees.

      4. Digital encoders that are not “absolute encoders” will default to a 0 position on power up. This is regardless of the actual motor position. Many motor systems will implement a physical limit switch. On power up they will slowly move to an end-point where the physical switch gets asserted by the mechanical system. When this is detected the encoder position is set to 0 which gives you a valid starting point.

      5. You’ll notice that there are a lot of things that have to be addressed with digital encoders. This is why many systems use stepper motors. You still need limit switches to to find your starting point, but once that is found you can just read the pot and send step signals to the motor.

      Hope some of this helps.

  11. christopher lowden says:

    Thank you so much for your insight. What niavely seemed to be relatively simple project seems to be my pandora’s box.
    The channel a&B reading I get back from the motor via the arduino for the motor is around 9000 clicks from the start point that is always zero when the system is turned on. If I understand you correctly, mapping between 1024 pot positions and the 9000 clicks makes for alot of discrepancy.
    I think that i am going to have to find an alternative way of making this project work. Many thanks again

    • Christopher,

      If feasible you could might consider using a potentiometer for feedback. This is sometimes not possible because of how the pot needs to mount to the output shaft. But a pot coupled to the output shaft would basically give you matching desired and actual position readings, and at power up the pot retains its position. This is what a hobby servo uses for position feedback.

      If you explore this route you need to make sure the motor is limited from rotating beyond the physical limits of the pot (or it will likely rip it apart). Or use a continuous rotation pot and write code to detect movement beyond the desired range. One gotcha when using a pot is that you need to initially set the motor position so when you connect the feedback pot it is “in position”. For example, you might move the motor to its mid-point of travel, and then adjust the pot to output half its voltage range, and then couple the pot to the shaft. You can’t just willy-nilly connect the pot. A second “gotcha” is that you need to make sure the feedback pot voltage increases when the motor moves in a positive direction. Otherwise the error signal in the PID algorithm has its sign reversed, and the motor will runaway trying to reduce the error only to cause the error to increase.

Speak Your Mind

*