Go Down

### Topic: Balancing robot for dummies (Read 291165 times)previous topic - next topic

#### kas

#15
##### Sep 28, 2010, 10:01 pm
Quote

Sure, processing code I used is from -> http://wiki.processing.org/w/Tom_Igoe_Interview

Hi Gibby623
Thanks for the link
I have purchased a copy of Tom's "Making Things Talk" and do recommend this book.

Quote

Kas I found a problem with my code which was I was updating the gyro angle referenced to the real estimated angle.
Code:
gyroAngle = angle + rollRate*dt

Code:
gyroAngle = prev_gyroAngle + rollRate*dt

unless you modify gyro_alpha in some other places,
gyro_alpha = prev_gyro_alpha + rollRate*0.011;
is equivalent to
gyro_alpha = gyro_alpha + rollRate*0.011;
prev_gyro_alpha = gyro_alpha;
You should't get a different result
In fact
prev_gyro_alpha = gyro_alpha;
should be deleted:

Code: [Select]
`void getGyroAlpha()    {  rollRate = ((rollADC - rollADC_offset)*Aref/1024)/0.0091;  gyro_alpha = gyro_alpha + rollRate*0.011;} `
I probably miss something  :-?

Quote

Please explain the 0.0091 coefficient

#### gibby623

#16
##### Sep 29, 2010, 12:03 pm
Kas, you are very right about the prev_gyro_alpha, but this still will not solve my issue... I will let you know if I find the issue.

Quote
Please explain the 0.0091 coefficient

As for the 0.0091, I have the sensor that offers the x4.5 pin which has a sensitivity og 9.1mV/degree/s instead of the 2.?mV/degrees/s

#### kas

#17
##### Sep 29, 2010, 12:15 pm
Quote
As for the 0.0091, I have the sensor that offers the x4.5 pin which has a sensitivity og 9.1mV/degree/s instead of the 2.?mV/degrees/s

Gibby623, please let me have a direct link to this sensor

#### gibby623

#18
##### Sep 29, 2010, 01:13 pm
http://www.sparkfun.com/commerce/product_info.php?products_id=9268

#### gioscarab

#19
##### Sep 30, 2010, 12:39 am
Take a look. To start a project like this I think you need the KISS approach. I start in this side of robotics with a very simple system, Look at this:
http://www.gioblu.com/tutorials/programmazione/102-che-cose-lalgoritmo-pid

Here to build it:
http://www.gioblu.com/tutorials/robotica/100-self-balancing-robot-per-noobs-p

here our code for mems:
http://www.gioblu.com/tutorials/programmazione/98-filtro-di-kalman-con-arduino
Community robotica / programmazione Arduino
www.gioblu.com
PJON multimaster communications bus system for Arduino and IOT https://github.com/gioblu/PJON

#### kas

#20
##### Sep 30, 2010, 10:06 amLast Edit: Sep 30, 2010, 11:18 am by kas Reason: 1
Hi gbm,
I like your bot, thanks for sharing your project
I have tried using servo motors without success
My bot was definitly too heavy (to much inertia) for the small servos
Servos are slow (60 RPM), large wheels is a plus to get an acceptable linear speed
Please let me have a link to the servos you used

Your write up regarding PID looks pretty interesting, but Google translate gives sometime funny results

Special note for other Balancing Robot makers
links to your beloved creations are welcomed in this thread

#### kas

#21
##### Oct 01, 2010, 04:56 pmLast Edit: Oct 02, 2010, 06:50 am by kas Reason: 1
[size=14] ** Balancing robot for dummies **[/size]

Part three: Kalman filtering

6 - So... where are we now ??

As a reminder, here is the pseudo code for the balancing process:
Code: [Select]
`loop                                        (fixed time loop 10ms = 100Hz)- sensors aquisition, smoothing and zeroing- Acc angle (quids) and Gyro rate (quids per second) calculation and scaling- Acc and Gyro data fusing though Kalman algorithm                          << here we are- PWM calculation using a PID control algorithm- PWM feeding to DC motorsendLoop`

7 - Data fusing

So, what do we get sofar ?? a noisy Acc angle and a drifting Gyro rate of rotation
Let's fuse them !!!
I have tried 2 filtering technologies:

- Complementary filter
This filter is rather easy to understand
I have seen several successfull implementation using complementary filters
I tried it and got mixed results, probably due to some non related design flaws
More details here: http://web.mit.edu/scolton/www/filter.pdf

- Kalman filter
Well over normal understanding , but... efficient
more details at http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/15   (Reply #24)
I modified a code snippet I found on the Web to produce an easy to use/self contained function
see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0  (Reply #14)

When rotating the board, make sure that GYRO_rate has the right sign:
GYRO_rate should be positive when rotating board toward +256 Quids (+180°)
and negative when rotating board toward -256 Quids (-180°)
This is another pitfall (been there, done that  )

The smoothing effect is quite impressive and is best viewed in graphing packages such as Processing or LabView

Watch it here http://www.youtube.com/v/H5Drlqv1t3s?
The red line is the raw ACC angle, the blue one is the filtered Signal

This is the updated code including Kalman filter:
Code: [Select]
`// Main module   K_bot angle    angles in Quids, 10 bit ADC -----------------------------// 7 - Data fusing, Kalman filter// Installation:// Create "Kalman" and "Sensors" tabs// Cut and paste the 2 modules in their respective tab// Save as "Kas_bot_angle"#include <math.h>#define   GYR_Y                 0                              // Gyro Y (IMU pin #4)#define   ACC_Z                 1                              // Acc  Z (IMU pin #7)#define   ACC_X                 2                              // Acc  X (IMU pin #9)int   STD_LOOP_TIME  =          9;             int sensorValue[3]  = { 0, 0, 0};int sensorZero[3]  = { 0, 0, 0 }; int lastLoopTime = STD_LOOP_TIME;int lastLoopUsefulTime = STD_LOOP_TIME;unsigned long loopStartTime = 0;int actAngle;int ACC_angle;int GYRO_rate;void setup() {  analogReference(EXTERNAL);                                            // Aref 3.3V  Serial.begin(115200);  calibrateSensors();}void loop() {  // ********************* Sensor aquisition & filtering *******************  updateSensors();  ACC_angle = getAccAngle();                                           // in Quids  GYRO_rate = getGyroRate();                                           // in Quids/seconds  actAngle = kalmanCalculate(ACC_angle, GYRO_rate, lastLoopTime);      // calculate filtered Angle// ********************* print Debug info *************************************  serialOut_labView();// *********************** loop timing control **************************  lastLoopUsefulTime = millis()-loopStartTime;  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);  lastLoopTime = millis() - loopStartTime;  loopStartTime = millis();}void serialOut_labView() {    Serial.print(actAngle + 512);        Serial.print(",");         // in Quids (0-512-1024)  Serial.print(ACC_angle + 512);       Serial.print("\n");}// --- Kalman filter module  ----------------------------------------------------------------------------    float Q_angle  =  0.001; //0.001    float Q_gyro   =  0.003;  //0.003    float R_angle  =  0.03;  //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;  } // --- Sensors Module ------------------------------------------------------------------------------------void calibrateSensors() {                                       // Set zero sensor values  long v;  for(int n=0; n<3; n++) {    v = 0;    for(int i=0; i<50; i++)       v += readSensor(n);    sensorZero[n] = v/50;  }                                                            // (618 - 413)/2 = 102.5    330/3.3 = x/1024  sensorZero[ACC_Z] -= 100;    //102;                          // Sensor: horizontal, upward}void updateSensors() {                                         // data acquisition  long v;  for(int n=0; n<3; n++) {    v = 0;    for(int i=0; i<5; i++)       v += readSensor(n);    sensorValue[n] = v/5 - sensorZero[n];  }}int readSensor(int channel) {  return (analogRead(channel));}int getGyroRate() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)  return int(sensorValue[GYR_Y] * 4.583333333);                 // in quid/sec:(1024/360)/1024 * 3.3/0.002)}int getAccAngle() {                        return arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256;    // in Quid: 1024/(2*PI))}int arctan2(int y, int x) {                                    // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm   int coeff_1 = 128;                                          // angle in Quids (1024 Quids=360°) <<<<<<<<<<<<<<   int coeff_2 = 3*coeff_1;   float abs_y = abs(y)+1e-10;                     float r, angle;     if (x >= 0) {     r = (x - abs_y) / (x + abs_y);     angle = coeff_1 - coeff_1 * r;   }  else {     r = (x + abs_y) / (abs_y - x);     angle = coeff_2 - coeff_1 * r;   }   if (y < 0)      return int(-angle);                     else            return int(angle);} `
We now have a nice filtered value reflecting the position of the bot:
Negative value when leaning forward
Positive value when forward
Zero when vertical, at equilibrium

Most of the sensitive job is done
Again, don't go further if you don't get an accurate, stable reading
This will save you a lot of frustration

Next part: PID Algorithm & (hopefully) DC motor control

#### osiwb123

#22
##### Oct 03, 2010, 09:00 am
This is a nice project thank you for sharing it, I am starting with my project and I will need your help  , I just order the parts.

#### kas

#23
##### Oct 03, 2010, 12:59 pm
Thanks osiwb123 for the kind words
If you are still able to perform this operation, please remove the Poll items you placed at the begining of this thread.
Please start a new thread for this pool

#### gibby623

#24
##### Oct 07, 2010, 09:11 am
Hey, haven't replied for a while. I found the issue with my gyro angle, but now my bot has been suffering from drifting... I have essentially gone back to the beginning...

#25

#### kas

#26
##### Oct 07, 2010, 06:28 pmLast Edit: Oct 07, 2010, 06:29 pm by kas Reason: 1
Hi Gibby623

Your boot _does_ balance and only need some tuning

Code: [Select]
`void getGyroAlpha(){  rollRate = ((rollADC - rollADC_offset)*Aref/1024)/0.0091;  /*if (rollRate < .5)   {   rollRate = 0;   }*/  gyro_alpha = prev_gyro_alpha + rollRate*0.011;  prev_gyro_alpha = gyro_alpha;} `
Did you introduced a dead band as per above ??
I tried adding dead band and got overshooting as per your video

PID tuning
You may have inadequate derivative action (not enough damping)
Try modifying "D", also experiment negative values

Finally, please share your code, I will load it in my bot implementation

#### gibby623

#27
##### Oct 08, 2010, 05:42 amLast Edit: Oct 08, 2010, 05:43 am by gibby623 Reason: 1

I will put up code soon, I have just re-written the whole code for a third time in 2 days!

And yes there is a dead band... not good?

#### kas

#28
##### Oct 08, 2010, 09:22 amLast Edit: Oct 08, 2010, 09:25 am by kas Reason: 1
Quote
Your bot is becoming quite stable

Quote
And yes there is a dead band... not good?
Try removing dead band and see for yourself (YMMV)
Also fine tune PID parameters. To avoid reloading code each time, parameters should be modified from the PC, using the serial monitor

Quote
I will put up code soon, I have just re-written the whole code for a third time in 2 days!
as soon as you post it, Iwill try it with my bot design

#### dumbified

#29
##### Oct 09, 2010, 06:54 pm
hi there , I have trouble searching for motors like yours. what are the factors that led you to choose this motor? I need to find a substitute but I do not know what to look out for. and they are rather expensive.

Go Up