Go Down

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

kas Sep 17, 2010, 05:46 pmLast Edit: Sep 17, 2010, 06:13 pm by kas Reason: 1
The 1st part of this thread is here:
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0
The previous thread's name was not relevant any more, so I decided to create this new topic.

I have been fascinated by balancing robots since years
My first implementation is here http://forums.parallax.com/showthread.php?t=103850&highlight=balancing
The bot used an IR sensor to maintain a constant distance to the floor
Since then, any attemp to make an IMU based robot, failed miserably
I read a lot and realized that I had taken the wrong path (Continuous rotation servos, sensor position, integer math, single power supply...)

I started again from scratch, ... and here it is    In this thread, I intend to step by step describe this project in an attemp to help newbys and save time in their quest for balance.
Describing and sharing projects is also the best way to learn from others. The second objective is to implement and propose various potential improvement: 12 bit ADC, wheel encoding, moving bot with RC control

More to come, stay tuned  8-)

gibby623 #1
Sep 17, 2010, 06:31 pm
Hey!!!

Well, I have just spent about 2 hours getting my angle estimations to graph on processing. Got it working and it is beautiful to see your data in a graph. My angle estimation is quick, accurate but still easily subject to translations.

Looking forward for this dummies tutorial!

kas #2
Sep 17, 2010, 09:54 pmLast Edit: Sep 18, 2010, 07:21 am by kas Reason: 1
Disclaimer:
I invented nothing
I read A LOT and went by trial and error

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

Part one: sensor, setup and fixed time loop

Quote

1 - sensor This is the IMU (Inertial Measurement Unit), 5DOF (5 Degrees Of Freedom) from Sparkfun
http://www.sparkfun.com/commerce/product_info.php?products_id=9268
This breakboard incorporates a IDG-500 dual-axis gyroscope and a triple axis ADXL335 accelerometer
Datasheets:
http://www.sparkfun.com/datasheets/Components/SMD/Datasheet_IDG500.pdf
We actually need only 3DOF (2 Acc and 1 Gyro axis)
I got an older model with a ADXL330/IDG-300 combo (remember I have been wandering around this project for two years )

2 - Setup The board setup is rather straight forward
The card needs a 3.3V supply (not 5V )
Green wire:  IMU pin#4 = GyroY to AI0
blue wire:   IMU pin#7 = AccZ  to AI1
Yellow wire: IMU pin#9 = AccX  to AI2
Also, for better ADC resolution, connect the 3.3V to the Arduino AREF
Position the Arduino supply jumper to "USB"

3 - Fixed timed loop
The main loop duration is critical for the filtering and PID modules
Code: [Select]
`int STD_LOOP_TIME = 9;             int lastLoopTime = STD_LOOP_TIME;int lastLoopUsefulTime = STD_LOOP_TIME;unsigned long loopStartTime = 0;void loop()  {// your code ...// *********************** loop timing control **************************  lastLoopUsefulTime = millis()-loopStartTime;  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);  lastLoopTime = millis() - loopStartTime;  loopStartTime = millis();}`
This loop runs 100 times per second (100 Hz)
Motor will receive an updated PWM value every 10 milliseconds

Loop timing is easily checked using a scope
just toggle one Digital Output within the loop: an easier way: add serialOut_timing() in the loop and watch the serial monitor

Code: [Select]
`void serialOut_timing() {static int skip=0;  if(skip++==5) {                         // display every 500 ms (at 100 Hz)                                  skip = 0;    Serial.print(lastLoopUsefulTime);    Serial.print(",");    Serial.print(lastLoopTime);          Serial.print("\n");  }}`

The first number is the real time needed to go through the loop
The second one is the final time loop, including the variable added delay

Next Part: data aquisition, zeroing and scaling

This presentation is rather time consuming;
English is not my native language (you may have notice the accent français in my writing)
I will pursue if sufficient interest is shown
also, if I go too slow, too fast or if I am just boring, just let me know Ro-Bot-X #3
Sep 18, 2010, 07:23 am
Keep going, I can't wait for more! This way I'll get mine working soon!

gibby623 #4
Sep 22, 2010, 05:10 am
When's the new installment? Hope all is well Kas!

granyte #5
Sep 22, 2010, 06:04 pm
keeep going this is quite interesting

kas #6
Sep 22, 2010, 09:10 pm
I am busy preparing part II
Hope to be ready by friday  8-)

kas #7
Sep 24, 2010, 07:12 pm
[size=14]          ** Balancing robot for dummies **[/size]

[size=14]Part two: sensors aquisition smoothing and zeroing, angle calculation[/size]

Forewords: Acc sensors are... noisy [size=14]1 - Accelerators response vs gravity[/size] taken from the ADXL330 data sheet

[size=14]2 - Zeroing sensor[/size]
Before aquiring data in the loop, sensors should be zeroed
This means that when the bot is stricly still and vertical, sensors should read "0"
except for the vertical axis (Acc_Z), which is sensing gravity (1g)
Zero mesurement is performed in setup:

Code: [Select]
`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;  }                                                              sensorZero[ACC_Z] -= 102;                         }`calibrateSensors() is a one off action, so we have time to average 3 X 50 measurements
Finally, gravity value is deducted to Acc_Z
For ADXL330/335: 1g = 330 mV (+- 10%)
ACC_Z correction for 10 bit ADC and 3.3V AREF: 330/3300*1024 = 102 (to be fine tuned later on)
//TODO for V2: before averaging, remove the 5 upper and lower values (abnormal noise) or find modal value (The mode of a set of numbers is the value that occurs most frequently)

[size=14]3 - Sensor aquisition[/size]

Code: [Select]
`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];  }}`Each sensor is pooled 5 time and averaged, zero value is then deducted
//TODO for V2: before averaging, remove the upper and lower values (noise)

[size=14]4 - Checking sensor data format[/size]

Code: [Select]
`// Main module   K_bot               angles in Quids, 10 bit ADC  -------------// 4 - Checking sensor data format    display raw sensors data             #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  = { 0, 0, 0};int sensorZero   = { 0, 0, 0}; int lastLoopTime = STD_LOOP_TIME;int lastLoopUsefulTime = STD_LOOP_TIME;unsigned long loopStartTime = 0;void setup() {  analogReference(EXTERNAL);                                   // Aref 3.3V  Serial.begin(115200);  delay(100);                                                  calibrateSensors();}void loop() {// ********************* Sensor aquisition & filtering *******************  updateSensors();// ********************* print Debug info *************************************  serialOut_raw();// *********************** loop timing control **************************  lastLoopUsefulTime = millis()-loopStartTime;  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);  lastLoopTime = millis() - loopStartTime;  loopStartTime = millis();}void serialOut_raw() {static int skip=0;  if(skip++==40) {                                                            skip = 0;    Serial.print("ACC_X:");           Serial.print(sensorValue[ACC_X]);              Serial.print("  ACC_Z:");         Serial.print(sensorValue[ACC_Z]);    Serial.print("  GYR_Y:");         Serial.println(sensorValue[GYR_Y]);  }}// 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;  }                                                            sensorZero[ACC_Z] -= 103;                        }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));}`
The sensors values vs position should read as follow:
Horizontal  ( 0° =  0 Quid )        ACC_X: 0         ACC_Z: XX      GYR_X: 0
Left side   (-90° = -256 Quid)    ACC_X: XX       ACC_Z: 0        GYR_X: 0
Right side (+90° = +256 Quid)  ACC_X:-XX       ACC_Z: 0        GYR_X: 0
Reversed  (180° = +512 Quid)  ACC_X: 0         ACC_Z:-XX      GYR_X: 0

For ADXL330/335,   XX value is around 100: Before going further, make sure you get that type of symetrical data
(this is one pitfall in this project )
It is now time to adjust sensorZero[ACC_Z] by adjusting "sensorZero[ACC_Z] -= 102"in order to have opposite/symetrical values (ie +103 and - 103) when going from 0 to 512 Quids (O° to 180°).

kas #8
Sep 24, 2010, 07:16 pmLast Edit: Sep 24, 2010, 07:17 pm by kas Reason: 1
- Continued -

[size=14]5 - angle and rate calculation[/size]
ACC_angle = getAccAngle();
the angle is obtained by using the arctangent of the two accelerometers readings.
The accelerometer values do not need to be scaled into actual units, but must be zeroed and have the same scale.

GYRO_rate = getGyroRate();
Determine gyro angular rate from raw analog values.
Gyro sensitivity is 2.0 mV/degrees/second
With the Arduino 10 bit ADC:
One ADC unit is 4.583333333 quid/sec (1.611328 degrees/sec)

Code: [Select]
`// Main module   K_bot angle    angles in Quids, 10 bit ADC -----------------------------// 5 - angle and rate calculation       display ACC_Angle and GYRO_rate#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  = { 0, 0, 0};int sensorZero  = { 0, 0, 0 }; int lastLoopTime = STD_LOOP_TIME;int lastLoopUsefulTime = STD_LOOP_TIME;unsigned long loopStartTime = 0;int ACC_angle;int GYRO_rate;void setup() {  analogReference(EXTERNAL);                                            // Aref 3.3V  Serial.begin(115200);  delay(100);                                                  calibrateSensors();}void loop() {// ********************* Sensor aquisition & filtering *******************  updateSensors();  ACC_angle = getAccAngle();                                 // in Quids    GYRO_rate = getGyroRate();                                 // in Quids per seconds // ********************* print Debug info *************************************  serialOut_sensor();// *********************** loop timing control **************************  lastLoopUsefulTime = millis()-loopStartTime;  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);  lastLoopTime = millis() - loopStartTime;  loopStartTime = millis();}void serialOut_sensor() {static int skip=0;  if(skip++==20) {                                                            skip = 0;    Serial.print(ACC_angle);          Serial.print(",");    Serial.print(GYRO_rate);          Serial.print("\n");  }}// 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);}`

You can check the (noisy) Acc angle and Gyro rate within the serial monitor
a better option is to display graphical data using LabView or Processing
I can share my code with Labview owners

Next part: Kalman filtering & PID Algorithm

Do I put to much detail ??
Should I go deeper ??
Let me know killersquirel11 #9
Sep 25, 2010, 09:56 pm
One thing that I would do is mount the batteries as far up as you can.  This (counterintuitively) make the 'bot easier to balance.

Quote
Do I put to much detail ??
Should I go deeper ??

You can never go into too much detail, at least as far as robots are concerned...  I really like this write-up so far

WanaGo #10
Sep 26, 2010, 04:36 am
Quote
This (counterintuitively)

I wouldnt say that... it does make sense. These are inverse pendulum's after all, so when you think about say a clocks pendulum, the weight is at the bottom. Since these are inverse the weight should be at the top. The motors weight is effectively null as its on the axis of rotation about the axle's.
Not debating anything you said obviously, but it does make sense.

Very good writeup - keep it up I have had all the bits of make one of these for about a year now, but I just havent gotten around to starting - too many other projects on the go sadly.

Good stuff

James

X-firm #11
Sep 26, 2010, 02:44 pm
I love this post I bean planing to make a balancing robot for years but this have given me the inspiration and knowledge to start a project..

I quite new to Arduino and Processing but I have ordered a Arduino with some parts to be able to start experimenting.

But until dose things arrive I have playing around with processing and made a meter to show for example a angel like kas did in his video that I think was made in LabView.

Heres the link to the example and the source code of my first try.
Meter_test : Built with Processing

Keep up the good work The balancing robot for dummies guide
http://www.x-firm.com/?page_id=145

wowmir #12
Sep 26, 2010, 07:28 pm
This is a great post, please keep going at a pace you are comfortable with, without hampering your other engagements.

Go into as much detail as you want, specially on the PID front.

granyte #13
Sep 27, 2010, 10:11 pm
keep going this post is great and verry usefull i plan on making a quad copter and was wonderuing how i would do the self balancing function as i never messed around gyro and accelerometer so your explaination are quite a good start

and your on a forum for DIY you can never go to much in detail

gibby623 #14
Sep 28, 2010, 02:19 am
Sure, processing code I used is from -> http://wiki.processing.org/w/Tom_Igoe_Interview
Code: [Select]
`/*  Serial Graphing Sketch  by Tom Igoe  Language: Processing  This sketch takes ASCII values from the serial port  at 9600 bps and graphs them.  The values should be comma-delimited, with a newline  at the end of every set of values.  The expected range of the values is between 0 and 1023.  Created 20 April 2005  Updated 27 June 2008*/ import processing.serial.*; int maxNumberOfSensors = 6;       // Arduino has 6 analog inputs, so I chose 6boolean fontInitialized = false;  // whether the font's been initializedSerial myPort;                    // The serial port float[] previousValue = new float[maxNumberOfSensors];  // array of previous valuesint xpos = 0;                     // x position of the graphPFont myFont;                     // font for writing text to the window void setup () {  // set up the window to whatever size you want:  size(800, 600);          // List all the available serial ports:  println(Serial.list());  // I know that the first port in the serial list on my mac  // is always my  Arduino or Wiring module, so I open Serial.list().  // Open whatever port is the one you're using.  String portName = Serial.list();  myPort = new Serial(this, portName, 9600);  myPort.clear();  // don't generate a serialEvent() until you get a newline (\n) byte:  myPort.bufferUntil('\n');  // create a font with the fourth font available to the system:  myFont = createFont(PFont.list(), 14);  textFont(myFont);  fontInitialized = true;  // set inital background:  background(0);  // turn on antialiasing:  smooth();} void draw () {  // nothing happens in the draw loop,  // but it's needed to keep the program running} void serialEvent (Serial myPort) {  // get the ASCII string:  String inString = myPort.readStringUntil('\n');  // if it's not empty:  if (inString != null) {    // trim off any whitespace:    inString = trim(inString);    // convert to an array of ints:    int incomingValues[] = int(split(inString, ","));    // print out the values    //  print("length: " + incomingValues.length + " values.\t");    if (incomingValues.length <= maxNumberOfSensors && incomingValues.length > 0) {      for (int i = 0; i < incomingValues.length; i++) {        // map the incoming values (0 to  1023) to an appropriate        // graphing range (0 to window height/number of values):        float ypos = map(incomingValues[i], 0, 1023, 0, height/incomingValues.length);        // figure out the y position for this particular graph:        float graphBottom = i * height/incomingValues.length;        ypos = ypos + graphBottom;        // make a black block to erase the previous text:        noStroke();        fill(0);        rect(10, graphBottom+1, 110, 20);        // print the sensor numbers to the screen:        fill(255);        int textPos = int(graphBottom) + 14;        // sometimes serialEvent() can happen before setup() is done.        // so you need to make sure the font is initialized before        // you text():        if (fontInitialized) {          text("Sensor " + i + ":" + incomingValues[i], 10, textPos);        }        // draw a line at the bottom of each graph:        stroke(127);        line(0, graphBottom, width, graphBottom);        // change colors to draw the graph line:        stroke(64*i, 32*i, 255);        line(xpos, previousValue[i], xpos+1, ypos);        // save the current value to be the next time's previous value:        previousValue[i] = ypos;      }    }    // if you've drawn to the edge of the window, start at the beginning again:    if (xpos >= width) {      xpos = 0;      background(0);    }    else {      xpos++;    }  }}`

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

Code: [Select]
`gyroAngle = prev_gyroAngle + rollRate*dt`

However now I get a value too small, approximately 1/3 of the right angle. I am using a simple if statement which waits until the dt > 0.01, which in my case is 0.011 and that is my constant dt. What am I doing wrong?

Here is the relevant code;
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; }`
Code: [Select]
`void loop(){  t = millis();  dt = (t - prev_t)/1000.0; // dt when read on serial monitor is constantly 0.011  if (dt > 0.01)  {    prev_t = t;    rollADC = analogRead(0);    yADC = analogRead(3);    zADC = analogRead(4);    getAccAlpha();    getGyroAlpha();    estimateAlpha();    actuate();  }}`

Go Up