 # Balancing robot for dummies

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

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!

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

__ ** Balancing robot for dummies **__

Part one: sensor, setup and fixed time loop

Reminder, please read http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0

1 - sensor This is the IMU (Inertial Measurement Unit), 5DOF (5 Degrees Of Freedom) from Sparkfun

This breakboard incorporates a IDG-500 dual-axis gyroscope and a triple axis ADXL335 accelerometer
Datasheets:

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

``````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

``````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 Keep going, I can't wait for more! This way I'll get mine working soon!

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

keeep going this is quite interesting

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

__ ** Balancing robot for dummies **__

Part two: sensors aquisition smoothing and zeroing, angle calculation

Forewords: Acc sensors are… noisy :o

1 - Accelerators response vs gravity

taken from the ADXL330 data sheet

2 - Zeroing sensor
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:

``````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)

3 - Sensor aquisition

``````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)

4 - Checking sensor data format

``````// 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°).

• Continued -

5 - angle and rate calculation
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)

``````// 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
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 : One thing that I would do is mount the batteries as far up as you can. This (counterintuitively) make the 'bot easier to balance.

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

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

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 :D

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.

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

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

``````/*
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().
// 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.

``````gyroAngle = angle + rollRate*dt
``````

Instead of…

``````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;

``````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;
}
``````
``````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();
}
}
``````

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.

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

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:

``````void getGyroAlpha()    {
rollRate = ((rollADC - rollADC_offset)*Aref/1024)/0.0091;
gyro_alpha = gyro_alpha + rollRate*0.011;
}
``````

I probably miss something :-? Please clarify

rollRate = ((rollADC - rollADC_offset)*Aref/1024)/0.0091;

Please explain the 0.0091 coefficient

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.

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

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

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 (use google translate)

here our code for mems: http://www.gioblu.com/tutorials/programmazione/98-filtro-di-kalman-con-arduino