Pages: [1] 2 3   Go Down
Author Topic: DC motor control with PID  (Read 83879 times)
0 Members and 2 Guests are viewing this topic.
0
Offline Offline
Sr. Member
****
Karma: 18
Posts: 401
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi,

For my final projects (balancing robot), I acquired 2X 350 RPM Pololu motors with integrated encoders
and a 14A dual motor driver
http://www.pololu.com/catalog/product/1443
http://www.pololu.com/catalog/product/708



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

Code:
// 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

Code:
// 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)  smiley-wink
Dallaby      http://letsmakerobots.com/node/19558#comment-49685
Bill Porter  http://www.billporter.info/?p=286

Anyone feeling like buildind a library ??
Enjoy
« Last Edit: June 23, 2013, 12:07:33 am by kas » Logged

0
Offline Offline
Sr. Member
****
Karma: 18
Posts: 401
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Finally, this source code moves the motor to a specific position, based on encoder counts number:
Code:
// 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

Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 33
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

0
Offline Offline
Newbie
*
Karma: 0
Posts: 4
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
« Last Edit: August 31, 2010, 02:31:58 pm by zalo » Logged

0
Offline Offline
Sr. Member
****
Karma: 18
Posts: 401
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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)
http://www.sparkfun.com/commerce/product_info.php?products_id=9268
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 ...  smiley-wink

The actual loop will look like that:
Code:
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:   http://en.wikipedia.org/wiki/Kalman_filter
PID conrol:      http://en.wikipedia.org/wiki/PID_controller
those documents are "hard theory" and do not need to be fully understood for implementing the project
Logged

Offline Offline
Full Member
***
Karma: 1
Posts: 200
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi kas,
what does "Acc" mean?.
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 33
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
what does "Acc" mean?.

Maybe Accelermeter?
Logged

0
Offline Offline
Sr. Member
****
Karma: 18
Posts: 401
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
« Last Edit: September 05, 2010, 09:24:48 am by kas » Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 9
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

how high does your little car? ;D
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 33
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

How are things going with your project Kas?
Logged

0
Offline Offline
Sr. Member
****
Karma: 18
Posts: 401
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
http://www.youtube.com/v/H5Drlqv1t3s?
« Last Edit: September 09, 2010, 12:01:37 pm by kas » Logged

Offline Offline
Full Member
***
Karma: 1
Posts: 200
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

0
Offline Offline
Sr. Member
****
Karma: 18
Posts: 401
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Code:
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
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 33
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

0
Offline Offline
Sr. Member
****
Karma: 18
Posts: 401
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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:
Code:
// 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
Logged

Pages: [1] 2 3   Go Up
Jump to: