DC motor control with PID

Hi,

For my final projects (balancing robot), I acquired 2X 350 RPM Pololu motors with integrated encoders
and a 14A dual motor driver

In an attempt to understand DC motor control, I developped two source codes

The first one is real basic, giving motion control:

  • forward(power)
  • backward(power)
  • stop()

power parameter beeing the PWM value (0-255) sent to the motor

// MD03A_Motor_basic
// Test MD03a / Pololu motor

#define InA1            10                  // INA motor pin
#define InB1            11                  // INB motor pin 
#define PWM1            6                   // PWM motor pin

void setup() {
  pinMode(InA1, OUTPUT);
  pinMode(InB1, OUTPUT);
  pinMode(PWM1, OUTPUT);
}

void loop() {
  motorForward(200);                        //(25%=64; 50%=127; 100%=255)
  delay(5000);

  motorStop();
  delay(2000);

  motorBackward(200);
  delay(5000);
}

void motorForward(int PWM_val)  {
  analogWrite(PWM1, PWM_val);
  digitalWrite(InA1, LOW);
  digitalWrite(InB1, HIGH);
}

void motorBackward(int PWM_val)  {
  analogWrite(PWM1, PWM_val);
  digitalWrite(InA1, HIGH);
  digitalWrite(InB1, LOW);
}

void motorStop()  {
  analogWrite(PWM1, 0);
  digitalWrite(InA1, LOW);
  digitalWrite(InB1, LOW);
}

The "full blown" gives PID control over speed parameter, pretty much as the "Cruse Control" feature found on modern cars.
When mechanicaly loaded, the system will increase PWM in an attempt to maintain "actual speed" at "target speed" level.
This is the only way to run at low speed (20 RPM) while maintaining a high torque
"Actual speed" is computed based upon the encoder informations, acquired in an interrupt routine.
Voltage ("low bat" warning) and current (overheating) are also continuously monitored
Speed and direction of rotation are adjustable through computer keyboard

// Test MD03a / Pololu motor with encoder
// speed control (PI), V & I display
// Credits:
//   Dallaby   http://letsmakerobots.com/node/19558#comment-49685
//   Bill Porter  http://www.billporter.info/?p=286
//   bobbyorr (nice connection diagram) http://forum.pololu.com/viewtopic.php?f=15&t=1923


#define InA1            10                      // INA motor pin
#define InB1            11                      // INB motor pin 
#define PWM1            6                       // PWM motor pin
#define encodPinA1      3                       // encoder A pin
#define encodPinB1      8                       // encoder B pin
#define Vpin            0                       // battery monitoring analog pin
#define Apin            1                       // motor current monitoring analog pin

#define CURRENT_LIMIT   1000                     // high current warning
#define LOW_BAT         10000                   // low bat warning
#define LOOPTIME        100                     // PID loop time
#define NUMREADINGS     10                      // samples for Amp average

int readings[NUMREADINGS];
unsigned long lastMilli = 0;                    // loop timing 
unsigned long lastMilliPrint = 0;               // loop timing
int speed_req = 300;                            // speed (Set Point)
int speed_act = 0;                              // speed (actual value)
int PWM_val = 0;                                // (25% = 64; 50% = 127; 75% = 191; 100% = 255)
int voltage = 0;                                // in mV
int current = 0;                                // in mA
volatile long count = 0;                        // rev counter
float Kp =   .4;                                // PID proportional control Gain
float Kd =    1;                                // PID Derivitave control gain


void setup() {
  analogReference(EXTERNAL);                            // Current external ref is 3.3V
  Serial.begin(115600);
  pinMode(InA1, OUTPUT);
  pinMode(InB1, OUTPUT);
  pinMode(PWM1, OUTPUT);
  pinMode(encodPinA1, INPUT); 
  pinMode(encodPinB1, INPUT); 
  digitalWrite(encodPinA1, HIGH);                      // turn on pullup resistor
  digitalWrite(encodPinB1, HIGH);
  attachInterrupt(1, rencoder, FALLING);
  for(int i=0; i<NUMREADINGS; i++)   readings[i] = 0;  // initialize readings to 0

  analogWrite(PWM1, PWM_val);
  digitalWrite(InA1, LOW);
  digitalWrite(InB1, HIGH);
}

void loop() {
  getParam();                                                                 // check keyboard
  if((millis()-lastMilli) >= LOOPTIME)   {                                    // enter tmed loop
    lastMilli = millis();
    getMotorData();                                                           // calculate speed, volts and Amps
    PWM_val= updatePid(PWM_val, speed_req, speed_act);                        // compute PWM value
    analogWrite(PWM1, PWM_val);                                               // send PWM to motor
  }
  printMotorInfo();                                                           // display data
}

void getMotorData()  {                                                        // calculate speed, volts and Amps
static long countAnt = 0;                                                   // last count
  speed_act = ((count - countAnt)*(60*(1000/LOOPTIME)))/(16*29);          // 16 pulses X 29 gear ratio = 464 counts per output shaft rev
  countAnt = count;                  
  voltage = int(analogRead(Vpin) * 3.22 * 12.2/2.2);                          // battery voltage: mV=ADC*3300/1024, voltage divider 10K+2K
  current = int(analogRead(Apin) * 3.22 * .77 *(1000.0/132.0));               // motor current - output: 130mV per Amp
  current = digital_smooth(current, readings);                                // remove signal noise
}

int updatePid(int command, int targetValue, int currentValue)   {             // compute PWM value
float pidTerm = 0;                                                            // PID correction
int error=0;                                  
static int last_error=0;                             
  error = abs(targetValue) - abs(currentValue); 
  pidTerm = (Kp * error) + (Kd * (error - last_error));                            
  last_error = error;
  return constrain(command + int(pidTerm), 0, 255);
}

void printMotorInfo()  {                                                      // display data
  if((millis()-lastMilliPrint) >= 500)   {                     
    lastMilliPrint = millis();
    Serial.print("SP:");             Serial.print(speed_req);  
    Serial.print("  RPM:");          Serial.print(speed_act);
    Serial.print("  PWM:");          Serial.print(PWM_val);  
    Serial.print("  V:");            Serial.print(float(voltage)/1000,1);
    Serial.print("  mA:");           Serial.println(current);

    if (current > CURRENT_LIMIT)               Serial.println("*** CURRENT_LIMIT ***");                
    if (voltage > 1000 && voltage < LOW_BAT)   Serial.println("*** LOW_BAT ***");                
  }
}

void rencoder()  {                                    // pulse and direction, direct port reading to save cycles
  if (PINB & 0b00000001)    count++;                // if(digitalRead(encodPinB1)==HIGH)   count ++;
  else                      count--;                // if (digitalRead(encodPinB1)==LOW)   count --;
}

int getParam()  {
char param, cmd;
  if(!Serial.available())    return 0;
  delay(10);                  
  param = Serial.read();                              // get parameter byte
  if(!Serial.available())    return 0;
  cmd = Serial.read();                                // get command byte
  Serial.flush();
  switch (param) {
    case 'v':                                         // adjust speed
      if(cmd=='+')  {
        speed_req += 20;
        if(speed_req>400)   speed_req=400;
      }
      if(cmd=='-')    {
        speed_req -= 20;
        if(speed_req<0)   speed_req=0;
      }
      break;
    case 's':                                        // adjust direction
      if(cmd=='+'){
        digitalWrite(InA1, LOW);
        digitalWrite(InB1, HIGH);
      }
      if(cmd=='-')   {
        digitalWrite(InA1, HIGH);
        digitalWrite(InB1, LOW);
      }
      break;
    case 'o':                                        // user should type "oo"
      digitalWrite(InA1, LOW);
      digitalWrite(InB1, LOW);
      speed_req = 0;
      break;
    default: 
      Serial.println("???");
    }
}

int digital_smooth(int value, int *data_array)  {    // remove signal noise
static int ndx=0;                                                         
static int count=0;                          
static int total=0;                          
  total -= data_array[ndx];               
  data_array[ndx] = value;                
  total += data_array[ndx];               
  ndx = (ndx+1) % NUMREADINGS;                                
  if(count < NUMREADINGS)      count++;
  return total/count;
}

Credits: (Inspiration rarely comes from the Vacuum) :wink:
Dallaby http://letsmakerobots.com/node/19558#comment-49685
Bill Porter SAGAR Update, New Motor Controller and Motors « The Mind of Bill Porter

Anyone feeling like buildind a library ??
Enjoy

Finally, this source code moves the motor to a specific position, based on encoder counts number:
moveMotor(FORWARD, speed, encoder_ticks); // direction, PWM, ticks number
The DC motor now beheaves as a stepper motor

// MD03A_Motor_basic + encoder

#define InA1            10                      // INA motor pin
#define InB1            11                      // INB motor pin 
#define PWM1            6                       // PWM motor pin
#define encodPinA1      3                       // encoder A pin
#define encodPinB1      8                       // encoder B pin

#define LOOPTIME        100                     // PID loop time
#define FORWARD         1                       // direction of rotation
#define BACKWARD        2                       // direction of rotation

unsigned long lastMilli = 0;                    // loop timing 
unsigned long lastMilliPrint = 0;               // loop timing
long count = 0;                                 // rotation counter
long countInit;
long tickNumber = 0;
boolean run = false;                                     // motor moves

void setup() {
  pinMode(InA1, OUTPUT);
  pinMode(InB1, OUTPUT);
  pinMode(PWM1, OUTPUT);
  pinMode(encodPinA1, INPUT); 
  pinMode(encodPinB1, INPUT); 
  digitalWrite(encodPinA1, HIGH);                      // turn on pullup resistor
  digitalWrite(encodPinB1, HIGH);
  attachInterrupt(1, rencoder, FALLING);
}

void loop() {
  moveMotor(FORWARD, 50, 464*2);                        // direction, PWM, ticks number
  delay(3000);
  moveMotor(BACKWARD, 50, 464*2);                           // 464=360°
  delay(3000);
}

void moveMotor(int direction, int PWM_val, long tick)  {
  countInit = count;    // abs(count)
  tickNumber = tick;
  if(direction==FORWARD)          motorForward(PWM_val);
  else if(direction==BACKWARD)    motorBackward(PWM_val);
}

void rencoder()  {                                    // pulse and direction, direct port reading to save cycles
  if (PINB & 0b00000001)    count++;                  // if(digitalRead(encodPinB1)==HIGH)   count_r ++;
  else                      count--;                  // if (digitalRead(encodPinB1)==LOW)   count_r --;
  if(run)  
    if((abs(abs(count)-abs(countInit))) >= tickNumber)      motorBrake();
}

void motorForward(int PWM_val)  {
  analogWrite(PWM1, PWM_val);
  digitalWrite(InA1, LOW);
  digitalWrite(InB1, HIGH);
  run = true;
}

void motorBackward(int PWM_val)  {
  analogWrite(PWM1, PWM_val);
  digitalWrite(InA1, HIGH);
  digitalWrite(InB1, LOW);
  run = true;
}

void motorBrake()  {
  analogWrite(PWM1, 0);
  digitalWrite(InA1, HIGH);
  digitalWrite(InB1, HIGH);
  run = false;
}

For the Pololu VNH2SP30 motor driver users:
What is your experience with the motor Amp monitoring (CS)?
I find the signal very noisy...

The balancing robot under construction

I will report on progress

This is off to a great start! I'll be interested in how it goes. How long do you have to complete this project?

This is a very cool project, I was thinking about doing something like it myself.

EDIT: Nevermind, I'm completely retarded for not knowing what an encoder is.

Anyway, I can't wait for progress.

Robot body is nearly finished, waiting for the 12V battery to be placed on the top

Angle measurement is performed with a 5 DOF IMU (2 axis Gyroscope + 3 axis accelerometer)

The sensors board is located between the wheels, near the rotation axis

Actually motor encoding is not required for balancing.
This feature can be added later on to prevent robot from drifting forward or backward

Here is the pseudo code for the balancing process:

loop (fixed time loop 10ms = 100Hz)

  • sensors aquisition, smoothing and zeroing
  • Acc angle (degrees) and Gyro rate (degrees per second) calculation and scaling
  • Acc and Gyro data fusing though Kalman algorithm
  • PWM calculation using a PID control algorithm
  • PWM feeding to DC motors
    endLoop

Before tackling next step, make sure that previous data is OK
we all know: Garbage in ... :wink:

The actual loop will look like that:

void loop() {
// ********************* Sensor aquisition & filtering *******************
  updateSensors();
  ACC_angle = angleDeg();                                                // in degrees
  GYRO_rate = rateDegPerSec();                                           // in degrees/seconds
  actAngle = kalmanCalculate(ACC_angle, GYRO_rate, lastLoopTime);        // calculate Absolute Angle

// *********************** Angle Control and motor drive *****************
  drive = updatePid(setPoint, actAngle);                                // PID algorithm

  if(actAngle>(setPoint-20) && actAngle<(setPoint+20))    Drive_Motor(drive); 
  else   Drive_Motor(0);                                                // stop motors if situation is hopeless
  
// ********************* Debug info **********************************  
//  SerialOut_timing();
//  SerialOut_raw();
//  SerialOut_sensor();
    SerialOut_labView();
  
// *********************** loop timing control **************************
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();
}

Further readings:

wheel encoder: http://www.mindspring.com/~tom2000/Delphi/Codewheel.html
Kalman filter: Kalman filter - Wikipedia
PID conrol: PID controller - Wikipedia
those documents are "hard theory" and do not need to be fully understood for implementing the project

Hi kas,
what does "Acc" mean?.

what does "Acc" mean?.

Maybe Accelermeter?

Accelerometers are used to sense both static (gravity) and dynamic (sudden starts/stops) acceleration - in m/s² or g.
Gyroscopes measure angular velocity, how fast something is spinning about an axis - in RPM, or degrees per second.
Accelerometer data are noisy, Gyros tend to drift; the combination (data fusing) gives usable information

See: http://www.sparkfun.com/commerce/tutorial_info.php?tutorials_id=167

how high does your little car? ;D

How are things going with your project Kas?

haha97: 33cm (13")
Gibby623: I received my 3800mAh 12V Battery, the robot will (should) balance this weekend

Here is a comparaison between the raw accelerator angle (red) and the Kalman filtered angle (blue)
The smoothing effect is impressive :o
P1020588 - YouTube?

Hi kas,
where did the graph display s/w (in the youtube video) come from?

This is a simple custom development in LabView.
The Arduino sends 2 integers separated by a comma

void serialOut_labView() {
  Serial.print(Angle + 512);           Serial.print(",");         // in Quids
  Serial.print(ACC_angle + 512);       Serial.print("\n");
}

Data can be sent alternatively to IDE serial monitor or to LabView

Angles are scaled in Quids (360° = 2 PI = 1024 Quids)
This unit is convenient and can be manipulated as integer while retaining a good resolution

Can you explain combining the accelerometer values and gyro values with a little more detail?

I know you are going to get swarmed for your code for the Kalman, but I am really struggling getting this to work. Can you detail how you went about this? Was the programming intensive? ...

Hi Gibby623, sorry, I just got your PM's
If your robot rolls about the x axis, you only need GYR_X, ACC_Y and ACC_Z
I notice that signal quality is much better when the sensor is upward, apparently, the IMU does not appreciate to be up side down
I will publish the full code pretty soon

here is the Kalman module:

// Kalman filter module

 float Q_angle  =  0.001;
 float Q_gyro   =  0.003;
 float R_angle  =  0.03; 

 float x_angle = 0;
 float x_bias = 0;
 float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;      
 float dt, y, S;
 float K_0, K_1;


  float kalmanCalculate(float newAngle, float newRate,int looptime) {
    dt = float(looptime)/1000;                                  
    x_angle += dt * (newRate - x_bias);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;
    
    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;
    
    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;
    
    return x_angle;
  }

The function parameters:

  • newAngle = ACC_angle (Accelerometers angle)
  • newRate = GYRO_rate (Gyro angle rate)
  • looptime = lastLoopTime (time between sensors measurement)

More info this week end, lets keep it public

It's balancing (at first try) :slight_smile: :slight_smile: :slight_smile:

See it live: Balancing robot - YouTube?

As soon as I install the battery on top (charger still not received), I will open a new thread with a more relevant title

This is freaking awesome!!! Congratulations!

I was so busy with my other projects that I neglected my balancing robot (I bought smaller motors and same Pololu wheels, they plug in directly on the motor shaft). I have a 2 axis accelerometer and a one axis gyro, do you think it will suffice or should I get a 5 axis IMU?

That is amazing, why is mine so bad... Sigh... What controller are you using for he wheels?

// Kalman filter module
float Q_angle = 0.001;
float Q_gyro = 0.003;
float R_angle = 0.03;

Hi Kas, awesome work on the project! My mate at uni was showing this to me today looks cool, great progress!

I am also working with an IMU a 6dof one and just wondering for your kalman filter above, how did you find the covariance matrices for measurement error and process error? lke what did you do and stuff?
Because im upto that aswell, i have my state space of the kalman filter, just dont understand how to get the covariance matrix.
Any help would be appreciated.

Thanks!!

Hi Ro-Bot-X, nice to see you around :wink:

I hope your robot will balance soon
Two axis accelerometer & one axis gyro are OK for the job
If you use separate breakout boards, make sure that axis are orthogonal, you may end up having the gyro board being perpendicular to the main board.
Also sensors units should match, don't fuse radian with deg/sec in your code
Finally the backlash we talked about is noticiable, but manageable

Please keep me informed of your progress, here
or there http://letsmakerobots.com/node/19558