Balancing robot for dummies

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

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

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()[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.

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 + rollRate0.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:

(use google translate)

Here to build it:

here our code for mems:

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

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

** Balancing robot for dummies **

Part three: Kalman filtering

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

As a reminder, here is the pseudo code for the balancing process:

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 motors
endLoop

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:

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 P1020588 - YouTube?
The red line is the raw ACC angle, the blue one is the filtered Signal

This is the updated code including Kalman filter:

// 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 :wink:

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

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.

Thank in advance

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

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

A sad attempt :frowning:

Hi Gibby623

Your boot does balance and only need some tuning

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

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?

Balanced-unstable.MOV - YouTube

Your bot is becoming quite stable :slight_smile:

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

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