Poll
Question: I will need help with my project ?
With the programing part - 47 (58%)
With the DC motors - 34 (42%)
Total Voters: 26

Pages: [1] 2 3 ... 16   Go Down
Author Topic: Balancing robot for dummies  (Read 142348 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Sr. Member
****
Karma: 18
Posts: 402
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

The 1st part of this thread is here:
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0
Please make sure you read it before going any further
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  smiley-grin smiley-grin smiley-grin


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

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.  smiley-wink
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-)
« Last Edit: September 17, 2010, 11:13:35 am by kas » Logged

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

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!
Logged

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

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/adxl335.pdf
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  :o)
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:
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:
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");
  }
}

You should read "2,10"
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   smiley-wink
« Last Edit: September 18, 2010, 12:21:54 am by kas » Logged

Toronto, Canada
Offline Offline
Full Member
***
Karma: 0
Posts: 144
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Keep going, I can't wait for more! This way I'll get mine working soon!
Logged


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

When's the new installment? Hope all is well Kas!
Logged

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

keeep going this is quite interesting
Logged

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

I am busy preparing part II
Hope to be ready by friday  8-)
Logged

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

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


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

Forewords: Acc sensors are... noisy :o


[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:
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:
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:
// 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[3]  = { 0, 0, 0};
int sensorZero[3]   = { 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 smiley-wink)
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°).

Logged

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

                                                                                                                                           - 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:
// 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[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 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
Gybby623, would you please post your Processing code ??

Next part: Kalman filtering & PID Algorithm

Do I put to much detail ??
Should I go deeper ??
Let me know  :smiley
« Last Edit: September 24, 2010, 12:17:21 pm by kas » Logged

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

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
Logged

NZ
Offline Offline
Sr. Member
****
Karma: 0
Posts: 390
Turtle in a hard shell
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 smiley

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
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 55
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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  smiley-grin
Logged

The balancing robot for dummies guide
http://www.x-firm.com/?page_id=145

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

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.
Logged

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

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
Logged

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

Sure, processing code I used is from -> http://wiki.processing.org/w/Tom_Igoe_Interview
Code:
/*
  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 6
boolean fontInitialized = false;  // whether the font's been initialized
Serial myPort;                    // The serial port
 
float[] previousValue = new float[maxNumberOfSensors];  // array of previous values
int xpos = 0;                     // x position of the graph
PFont 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()[0].
  // Open whatever port is the one you're using.
  String portName = Serial.list()[0];
  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()[3], 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:
gyroAngle = angle + rollRate*dt

Instead of...
Code:
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:
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:
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();
  }
}

Logged

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