Arduino Forum

Forum 2005-2010 (read only) => General => Exhibition => Topic started by: kas on Aug 21, 2010, 12:00 pm

Title: DC motor control with PID
Post by: kas on Aug 21, 2010, 12:00 pm
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

(http://i.imgur.com/uwui8az.jpg) (http://imgur.com/uwui8az)

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

Anyone feeling like buildind a library ??
Enjoy
Title: Re: DC motor control with PID
Post by: kas on Aug 28, 2010, 04:32 pm
Finally, this source code moves the motor to a specific position, based on encoder counts number:
 [size=12]moveMotor(FORWARD, speed, encoder_ticks);[/size]               // direction, PWM, ticks number
The DC motor now beheaves as a stepper motor

Code: [Select]
// 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
(http://imgur.com/CQbTH.jpg) (http://imgur.com/CQbTH.jpg)

I will report on progress

Title: Re: DC motor control with PID
Post by: gibby623 on Aug 31, 2010, 08:35 am
This is off to a great start! I'll be interested in how it goes. How long do you have to complete this project?
Title: Re: DC motor control with PID
Post by: zalo on Aug 31, 2010, 09:30 pm
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.
Title: Re: DC motor control with PID
Post by: kas on Sep 04, 2010, 08:01 pm
Robot body is nearly finished, waiting for the 12V battery to be placed on the top

(http://imgur.com/fzZh1.jpg) (http://imgur.com/fzZh1.jpg)

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 ...  ;)

The actual loop will look like that:
Code: [Select]
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
Title: Re: DC motor control with PID
Post by: novice on Sep 05, 2010, 06:15 am
Hi kas,
what does "Acc" mean?.
Title: Re: DC motor control with PID
Post by: gibby623 on Sep 05, 2010, 10:47 am
Quote
what does "Acc" mean?.


Maybe Accelermeter?
Title: Re: DC motor control with PID
Post by: kas on Sep 05, 2010, 03:35 pm
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
Title: Re: DC motor control with PID
Post by: zsw3979 on Sep 06, 2010, 03:11 pm
how high does your little car? ;D
Title: Re: DC motor control with PID
Post by: gibby623 on Sep 08, 2010, 12:14 am
How are things going with your project Kas?
Title: Re: DC motor control with PID
Post by: kas on Sep 09, 2010, 06:40 pm
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?
Title: Re: DC motor control with PID
Post by: novice on Sep 10, 2010, 02:27 am
Hi kas,
where did the graph display s/w (in the youtube video) come from?
Title: Re: DC motor control with PID
Post by: kas on Sep 11, 2010, 07:23 am
This is a simple custom development in LabView.
The Arduino sends 2 integers separated by a comma

Code: [Select]
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
Title: Re: DC motor control with PID
Post by: gibby623 on Sep 11, 2010, 07:40 am
Can you explain combining the accelerometer values and gyro values with a little more detail?
Title: Re: DC motor control with PID
Post by: kas on Sep 11, 2010, 08:47 am
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: [Select]
// 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
Title: Re: DC motor control with PID
Post by: kas on Sep 14, 2010, 11:50 am
It's balancing (at first try)  :) :) :)

(http://imgur.com/gb71m.jpg)

See it live:  http://www.youtube.com/v/pC6yeyDunJg?

As soon as I install the battery on top (charger still not received), I will open a new thread with a more relevant title
Title: Re: DC motor control with PID
Post by: Ro-Bot-X on Sep 14, 2010, 01:45 pm
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?
Title: Re: DC motor control with PID
Post by: gibby623 on Sep 15, 2010, 03:59 am
That is amazing, why is mine so bad... Sigh... What controller are you using for he wheels?
Title: Re: DC motor control with PID
Post by: thinkaneer on Sep 15, 2010, 07:12 am
[glow]// Kalman filter module

float Q_angle  =  0.001;
float Q_gyro   =  0.003;
float R_angle  =  0.03;[/glow]

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!!
Title: Re: DC motor control with PID
Post by: kas on Sep 15, 2010, 09:17 am
Hi Ro-Bot-X, nice to see you around  ;)

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
Title: Re: DC motor control with PID
Post by: kas on Sep 15, 2010, 11:50 am
@Gibby623

>>What controller are you using for he wheels?
Not sure I understand your question, please elaborate
If you mean motor driver,here it is: http://www.pololu.com/catalog/product/708

>>why is mine so bad... Sigh...
I understand your frustration,  ;) I have been pursuing this quest nearly for 2 years
Please post photos of your bot, together with the code, I will look for possible obvious reasons
Title: Re: DC motor control with PID
Post by: zsw3979 on Sep 15, 2010, 04:49 pm
i will do this later,if i success[ch65292]i'll show my pictures.  
i  got  a  question ,why do you put the battery on the top[ch65311]
i think that if you put it  on  the  bottom,you  can  controll  the  balance better.
Title: Re: DC motor control with PID
Post by: keeper63 on Sep 15, 2010, 07:38 pm
Quote
i think that if you put it  on  the  bottom,you  can  controll  the  balance better.


I know nothing about balancing robots, but I would tend to think having the mass at the top would be easier to balance. Try to balance a pencil on your hand, then try it with a weight on top (blob of clay), then a weight near the hand - you'll find its easier with the weight at the top. I imagine the same would be true for a robot (ie, inverted pendulum)...

:)
Title: Re: DC motor control with PID
Post by: kas on Sep 15, 2010, 08:43 pm
Quote
Quote:

i think that if you put it  on  the  bottom,you  can  controll  the  balance better.

I know nothing about balancing robots, but I would tend to think having the mass at the top would be easier to balance. Try to balance a pencil on your hand, then try it with a weight on top (blob of clay), then a weight near the hand - you'll find its easier with the weight at the top. I imagine the same would be true for a robot (ie, inverted pendulum)...


Your are right cr0sh
the robot acts as an inverted pendulum and works better when the weight is high (it increases intertia and allows more reaction time).
Try to balance a broom in the palm of your hand and see which side is easier to balance.
However, PID parameters need different tuning
Title: Re: DC motor control with PID
Post by: kas on Sep 15, 2010, 09:07 pm
Quote
// 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!!


Kalman filter module works pretty well but is still a Black Box for me
I tried hard to understand and finally gave up.  :o

This code is a modified version from the AeroQuad project from Ted Carancho
http://code.google.com/p/aeroquad/

I spent some hours playing with Q_angle, Q_gyro and R_angle, and finally reverted to the original parameters

Some additional lectures for the brave:
http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf
http://www.cs.unc.edu/~welch/kalman/index.html#Anchor-Rudolph-6296
http://forum.sparkfun.com/viewtopic.php?t=6186
Title: Re: DC motor control with PID
Post by: josev on Sep 16, 2010, 06:50 am
Kas,

I am confused, on the let's make Robots site, you stated that thanks to Dallaby your robot balances. Now,  Dallaby uses a complementary filter. You mention you are using a Kalman filter. So, is this the case, if so what is the contribution from Dallaby?

I am  asking because I am trying to start similar project and not sure whether to spend more time trying to be able to run the kalman routines or go ahead with the complementary filter. Several sites talks very good about this simpler approach.

Thanks !
Title: Re: DC motor control with PID
Post by: gibby623 on Sep 16, 2010, 05:25 pm
Kas,
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284650649/0#0

How do you add photos?
Title: Re: DC motor control with PID
Post by: kas on Sep 16, 2010, 05:26 pm
Hi josev,

This is a long story
I made several unsuccessfull attemps to build an acceptable balancing bot, both with Kalman or complementary filter; I finally gave up 6 months ago.
Reading, Dallaby blog gave me new ideas for the project, mainly for hardware (I was using continuous rotation servos).
Dallaby uses a single axis accelerometer which is OK small angles, together with a very simple and efficient code.
I chose the full blown path, but again, without visiting his site, I wouldn't be writing to you right now.  ::)

The above Kalman code is really convenient and self contained
Feed it with Acc angle, Gyro speed and loop time, it will spit back the filtered angle right away
Just make sure that data are scaled and your loop time is accurate/constant,
mine is running 100 Hz, see above "loop timing control".

Good luck, keep us informed
Title: Re: DC motor control with PID
Post by: kas on Sep 16, 2010, 05:37 pm
Quote
Kas,
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284650649/0#0

How do you add photos?


Easy  ;)
- Go to http://imgur.com/
- Upload you photo(s)
- copy the "Message board" line
- paste it here
Title: Re: DC motor control with PID
Post by: kas on Sep 16, 2010, 06:13 pm
Gibby623,
A bit dubious about your PID function
Why do you have "PID" and "PID_comp"?? is "a" a tuning factor (as my "K") ??
I would have expected PID to be positive or negative according to the bot being tilted forward or backward
Also if your loop is fixed time, you can omit the dt factor

anycase, here is my PID code:
Code: [Select]

float K = 1.4;
int   Kp = 3;                      
int   Ki = 1;                  
int   Kd = 6;  
int last_error = 0;
int integrated_error = 0;
int pTerm = 0, iTerm = 0, dTerm = 0;

int updatePid(int targetPosition, int currentPosition)   {
 int error = targetPosition - currentPosition;
 pTerm = Kp * error;
 integrated_error += error;                                      
 iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
 dTerm = Kd * (error - last_error);                            
 last_error = error;
 return -constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}

This function accepts 2 parameters
- target position (normally = 0)
- current position = the angle obtained from Kalman or complementary filter
the function returns an integer value, between -255 and +255

Please let me know which Acc and Gyro models are buit on your specific Sparkfun board
Title: Re: DC motor control with PID
Post by: gibby623 on Sep 17, 2010, 03:25 am
Quote
Why do you have "PID" and "PID_comp"?? is "a" a tuning factor (as my "K") ??

Yes, one motor responses quicker/more than the other. I know this is sad, I didn't know it accepts -255 to 255. I though it was only 0-255.

Quote
Also if your loop is fixed time, you can omit the dt factor

I am unsure how you can exclude dt, my dt is not constant. How do you make it constant?

How did you find your P, I and D values? Well, their gains, kp, ki, kd?

Quote
Please let me know which Acc and Gyro models are buit on your specific Sparkfun board

I am using the generic 5DOF sensor which has a ADXL335 and IDG500, I think.

Forgot to add, I uploaded my pictures in my topic, to avoid cluttering your topic with my ugly system
Title: Re: DC motor control with PID
Post by: kas on Sep 17, 2010, 01:56 pm
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284650649/0#0
Your bot is really sophisticated, you should have spent a lot of time on this project

Quote
Is there any reason why the gyro is placed at the bottom? Intuitively I would think it was better to place it near the 'top'.

Feel free to correct me, I read somewhere it eliminates uncertainty with the readings.
You are right, we want to measure gravity angles, not movements.
IMU board should be located near the weels (axe of rotation) to reduce noise sensors

Quote

Hardware: 2 x 29:1 12V Gearmotors,  SN754410NE H-Bridge, Arduino Nano 3.0, Sparkfun 5DOF IMU.
For the expensive parts (IMU, motors), You have the right hardware :)

Quote
Yes, one motor responses quicker/more than the other. I know this is sad, I didn't know it accepts -255 to 255. I though it was only 0-255.
My left motor is 10% slower than the left one, that the nature of the beasts, this is adjusted within function "Drive_Motor(int torque)" in the Motor Module, this adjustment is not PID related.
The motors accept only 0 to 255, the sign determines the direction of rotation
Again, this will be treated in "Drive_Motor(int torque)"


Gibby663, I suspect that your signal aquisition and processing is not adequate.
For you and possibly additional lurkers, I am willing to start from the very begining and rebuild the bot together
I am sure you already know most of the info to come, just look for the details
For convenience, you will have to remove you breadboard from the bot (I did)

Ready ???  OK!!!

Disclaimer:
I invented nothing
I read A LOT and went by trial and error



[size=16]      ** Balancing robot for dummies **[/size]

Part one: sensor, setup and fixed time loop

Stay tuned, I will open, later this afternoon, a new thread with a more relevant title than "DC motor control with PID".  8-)
Title: Re: DC motor control with PID
Post by: gibby623 on Sep 17, 2010, 03:18 pm
Quote
Gibby663, I suspect that your signal aquisition and processing is not adequate.

I suspect that too!!!

Quote
Your bot is really sophisticated, you should have spent a lot of time on this project

What do you mean sorry? It is rather generic though; IMU, H-bridge, microcontroller...

Quote
For convenience, you will have to remove you breadboard from the bot (I did)

Do you mean I should??? I don't have much time to solder everything to PCB. Does the breadboard introduce a lot of noise?

Thanks for taking some time to help :)
Title: Re: DC motor control with PID
Post by: kas on Sep 17, 2010, 06:00 pm
This is the new topic I just opened for further discussions
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/0#0

Please do not post anymore in this thread

Thanks
Title: Re: DC motor control with PID
Post by: Onions on Nov 26, 2010, 10:29 pm
I'd love to build smething like this, only I don't have the money. (or intelligence!)
Title: Re: DC motor control with PID
Post by: novice on Nov 28, 2010, 07:49 am
Quote
I'd love to build smething like this, only I don't have the money. (or intelligence!)

Begin with a small project (or part of the project) and then advance slowly to more advanced projects.
Title: Re: DC motor control with PID
Post by: Onions on Dec 12, 2010, 07:47 pm
I am planning on building a similar robot soon, so I was wondering where you got the wheels? (A UK supplier would be nice!)
Title: Re: DC motor control with PID
Post by: kas on Dec 13, 2010, 09:51 am
Hi onions,

This thread is not active anymore, please join us here: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418