Arduino Forum

Forum 2005-2010 (read only) => General => Exhibition => Topic started by: on Jan 01, 1970, 01:00 am

Poll
Question:
Option 1: With the programing part votes: 47
Option 2: With the DC motors votes: 34
Title: Balancing robot for dummies
Post by: kas on Sep 17, 2010, 05:46 pm
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

(http://imgur.com/gb71m.jpg)
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-)
Title: Re: Balancing robot for dummies
Post by: gibby623 on 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!
Title: Re: Balancing robot for dummies
Post by: kas on Sep 17, 2010, 09:54 pm
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
Reminder, please read http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0


1 - sensor
(http://imgur.com/29N4c.jpg)

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(http://imgur.com/N6Kc6.jpg)
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: [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:

(http://imgur.com/t1Wt7.jpg)

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");
 }
}


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   ;)
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Sep 18, 2010, 07:23 am
Keep going, I can't wait for more! This way I'll get mine working soon!
Title: Re: Balancing robot for dummies
Post by: gibby623 on Sep 22, 2010, 05:10 am
When's the new installment? Hope all is well Kas!
Title: Re: Balancing robot for dummies
Post by: granyte on Sep 22, 2010, 06:04 pm
keeep going this is quite interesting
Title: Re: Balancing robot for dummies
Post by: kas on Sep 22, 2010, 09:10 pm
I am busy preparing part II
Hope to be ready by friday  8-)
Title: Re: Balancing robot for dummies
Post by: kas on 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 :o


[size=14]1 - Accelerators response vs gravity[/size]

(http://imgur.com/1Kzh6.jpg)
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[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:

(http://imgur.com/SWlGr.jpg)

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

Title: Re: Balancing robot for dummies
Post by: kas on Sep 24, 2010, 07:16 pm
                                                                                                                                          - 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[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  ::)
Title: Re: Balancing robot for dummies
Post by: killersquirel11 on 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
Title: Re: Balancing robot for dummies
Post by: WanaGo on 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
Title: Re: Balancing robot for dummies
Post by: X-firm on 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 (http://www.x-firm.com/processing/Meter_test/)

Keep up the good work  :D
Title: Re: Balancing robot for dummies
Post by: wowmir on 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.
Title: Re: Balancing robot for dummies
Post by: granyte on 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
Title: Re: Balancing robot for dummies
Post by: gibby623 on 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 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: [Select]
gyroAngle = angle + rollRate*dt

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


Title: Re: Balancing robot for dummies
Post by: kas on 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

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:

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

I probably miss something  :-?
Please clarify

Quote

 rollRate = ((rollADC - rollADC_offset)*Aref/1024)/0.0091;
Please explain the 0.0091 coefficient
Title: Re: Balancing robot for dummies
Post by: gibby623 on 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

Title: Re: Balancing robot for dummies
Post by: kas on 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
Title: Re: Balancing robot for dummies
Post by: gibby623 on Sep 29, 2010, 01:13 pm
http://www.sparkfun.com/commerce/product_info.php?products_id=9268
Title: Re: Balancing robot for dummies
Post by: gioscarab on 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
(use google translate)

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
Title: Re: Balancing robot for dummies
Post by: kas on Sep 30, 2010, 10:06 am
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  :)
Title: Re: Balancing robot for dummies
Post by: kas on Oct 01, 2010, 04:56 pm
      [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 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:

- 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 :o :o , but... efficient
Please see http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
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

(http://imgur.com/itNlc.jpg)
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
Title: Re: Balancing robot for dummies
Post by: osiwb123 on 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.

Thank in advance
Title: Re: Balancing robot for dummies
Post by: kas on 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  ;)
Title: Re: Balancing robot for dummies
Post by: gibby623 on 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...
Title: Re: Balancing robot for dummies
Post by: gibby623 on Oct 07, 2010, 03:43 pm
A sad attempt :(
http://www.youtube.com/watch?v=ByC_QR3uLIQ
Title: Re: Balancing robot for dummies
Post by: kas on Oct 07, 2010, 06:28 pm
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
Title: Re: Balancing robot for dummies
Post by: gibby623 on Oct 08, 2010, 05:42 am
http://www.youtube.com/watch?v=wLFRkj4HIG0

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?
Title: Re: Balancing robot for dummies
Post by: kas on Oct 08, 2010, 09:22 am
Quote
http://www.youtube.com/watch?v=wLFRkj4HIG0
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
Title: Re: Balancing robot for dummies
Post by: dumbified on 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. :(
Title: Re: Balancing robot for dummies
Post by: kas on Oct 10, 2010, 08:28 am
(http://imgur.com/c3APk.jpg)
(http://imgur.com/890T7.jpg)

http://www.pololu.com/catalog/product/1443

Pros:
- powerfull (110 oz.in)
- solid design (metal gears)
- fast (350RPM, don't settle for less than 200RPM)
- integrated, efficient encoders (464 counts per wheel rotation)
- acceptable price (40 bucks, including encoder)

Cons:
- noticiable backlash (as with all spur gearmotors)

Planetary gearboxes are supposed to have no backlash
I have still to identify a planetary geared equivalent motor (speed, power, encoder)
any links ???
Title: Re: Balancing robot for dummies
Post by: kas on Oct 16, 2010, 11:49 am
I am currently integrating encoders data in order to reduce forward/backward drift

Wandering around for new ideas, I came across this one:
http://www.youtube.com/watch?v=Kd2kJxBkPmk
I have yet to see a more stable bot  :o  :o
Lets hope this guy will open a blog soon for documenting his project
Congratulation beautifulsmall  :)
Title: Re: Balancing robot for dummies
Post by: kas on Oct 22, 2010, 03:03 pm
      [size=14] ** Balancing robot for dummies **[/size]


[size=14]Part four: PID control[/size]

                          (http://imgur.com/0banY.png)

Forewords: Control strategy is a whole univers, let's keep it simple  ;)


[size=14]8 - Possible control strategies[/size]

error = Actual value - Set point

[size=12] - ON/OFF control[/size]
This strategy in used for electric heaters with mechanical thermostat    
 
Code: [Select]

pseudo code

if actTemp < SetPoint      Power = ON
else                  Power = OFF


Code for a basic (static) balancing robot
Set point = 0
error = actAngle
 
Code: [Select]

pseudo code

If actAngle = 0       Motors stop
else if actAngle <0      Motor full forward (PWM 256)
else if actAngle >0      Motor full backward (PWM 256)

Don't try it, it just doesn't work; let's be more sophisticated.


[size=12]- PID control (Proportional Integral Derivative)[/size]
This control is the sum of 3 actions  

Proportional action:
The proportional term makes a change to the output that is proportional to the current error value.
 
Code: [Select]
pTerm = Kp * error
A high value will cause a hard counter reaction to a change in attitude.
Above a certain value the system will start to oscillate, the ideal spot is just below this point.
You cannot ballance a robot with a proportional value of 0.
Proportional control only is much better than pure ON/OFF, but the Bot will keep oscillating and finally fall on the floor

Derivative action:
This action is proportional to the error variation
With a fixed time loop, time may be omited:
 
Code: [Select]
dTerm = Kd * (error - last_error);                            
 last_error = error;

The Derivative portion calculates how fast a change in angle occurs.
With ballancing bot, these values are usually used as a damping factor, meaning negative signs in the formula.
Without the derivative action, there is no damping to dissipate the energy of the system.  
With only proportional control, any input to the system will cause an undamped oscillation that will continue for ever.
If you are an electrical engineer, think of how an RLC circuit differs from an LC circuit
The derivative term works the fastest, because it detects change in rate before even a significant error appears.

Integral action:
 
Code: [Select]
integrated_error += error;                                      
 iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
GUARD_GAIN sets up a limit for iTerm.

The integral term allows a precise landing at setpoint, it is known as "steady state error".
It is the only term that keeps changing no matter how small the error remains.
You're basically telling the controller, "if you aren't quite getting to angle, keep increasing PWM over a period of time until you get there".

PID control is the sum of those 3 actions
 
Code: [Select]
int updatePid(int targetPosition, int currentPosition)   {
 int error = targetPosition - currentPosition;
 pTerm = Kp * error;
 integrated_error += error;                                      
 iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
 dTerm = Kd * (error - last_error);                            
 last_error = error;
 return -constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}


The best PID analogy I found is the spring car suspension:

                         (http://imgur.com/lrDaC.jpg)

kp is the strength of the spring.  
kd is the amount of damping needed to stop oscillations due to kp.

Proportional reacts to a measure of the present error.
Derivative reacts as a prediction of the future error,
Integral adjustment is based on past errors

PID tuning:
For manual tuning, set all three constants to zero, then turn up Kp until oscillation occurs.  
Then turn up Kd until oscillation disappears.  Adjust Kd until the system is critically damped, i.e. there's no overshoot.  
Then increase Ki until the steady-state error goes to zero in a reasonable time.

Further readings:
http://en.wikipedia.org/wiki/PID_controller
http://www.jashaw.com/pid/tutorial/

[size=14]Next part: DC motor control and final complete code (Version 1)[/size]

Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Oct 23, 2010, 09:13 pm
first post in Arduino,may well be teaching granny to suck eggs,  been making a 2-wheel balancing robot for 12months now. earwig robot on youtube, The BMA180, arctan2, combined with the ADXR150 gyro(10-bit) gives a 1/16 of a degree resolution. Nice.complementary filter. After Hours spent tuning pid values the best solution I found was to have a timer giving a step change to the motor PID, the step level was interactive as were the pid values, this way you can see whare the Buzz and unstable parameters are. The balance equation,
vert_error+base_vel+body_vel+position_error.
if vert error is initially off, the robot drifts until the pos error cancels out and the bot oscillates around a + or -pos offset , this can be used to calibrate the true vertical.
trying to map now and getting lost.
i wouldn't be this far on without the help of others :o
Title: Re: Balancing robot for dummies
Post by: kas on Oct 24, 2010, 10:28 am
Hi beautifulsmall, welcome to this forum  :) :)

As already mentioned, you built the more stable Bot I have ever seen (I saw a lot those last 2 years  ;))


(http://imgur.com/SyBdi.jpg)
http://www.youtube.com/watch?v=Kd2kJxBkPmk

I am really impressed by the lean back motion when pushed

My Version 1.0 KasBot doesn't yet take advantage of the encoder wheels;
It is stable and can balance forever, however it may drift forward/backward.
This is perfectly normal as the bot is not yet position aware
The idea is to inject encoder data in the PID loop:

Code: [Select]
KasBot V2 alpha

int updatePid(int targetPosition, int currentPosition)
 {
   int error = targetPosition - currentPosition;
   pTerm = Kp * error;                             // error: angle
   integrated_error += error;                                      
   iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
   dTerm = Kd * (error - last_error);                            
   last_error = error;
   xpTerm = K2p * count;                               // count: encoder tics
   xdTerm = K2d * (count - last_count);                          
   last_count = count;
   return -constrain(K*(pTerm + iTerm + dTerm + xpTerm + xdTerm), -255, 255);
 }

For encoders, I use speed proportional (K2p) and derivative (K2d), no integral
It does help somehow, forward/backward drift is reduced, but I am still far from what you achieved.

Code: [Select]
vert_error+base_vel+body_vel+position_errorNot sure to understand  :-?
 vert_error = angle
 base_vel = angle (derivative)
 body_vel = encoder (derivative)
 position_error = encoder
please confirm

Please let us have additional details
- forward/backward drift suppession
- lean back

finally, please post relevant code or speudo code
Title: Re: Balancing robot for dummies
Post by: X-firm on Oct 26, 2010, 07:54 pm
Now I have gotten the parts and will start building a jig to be able to test the parts.

I have started thinking about how to build the robot and especialy where to place the accelerometer and the gyro,

What I understand I need to have the gyro X-axis in the rotation centre of the wheels but how is it with the accelerometer. when its reading acceleration couldn't I place it where ever I want on the robot?

More info about the project:
http://www.x-firm.com/?cat=6
Title: Re: Balancing robot for dummies
Post by: T.J.L. on Oct 26, 2010, 10:10 pm
Quote
I have started thinking about how to build the robot and especialy where to place the accelerometer and the gyro,

What I understand I need to have the gyro X-axis in the rotation centre of the wheels but how is it with the accelerometer. when its reading acceleration couldn't I place it where ever I want on the robot?


Could someone please explain me why the gyro should be in the centre of rotation. As far is I understand gyro measures angular velocity, which is exactly the same in every point of a rigid body.

I think the average acceleration would be the same in every point of the robot. When the robot starts leaning to some direction the upper part of the robot accelerates slowly and when it corrects the position the lower part accelerates rapidly.
Title: Re: Balancing robot for dummies
Post by: kas on Oct 27, 2010, 08:58 am
Quote
Quote
I have started thinking about how to build the robot and especialy where to place the accelerometer and the gyro,

What I understand I need to have the gyro X-axis in the rotation centre of the wheels but how is it with the accelerometer. when its reading acceleration couldn't I place it where ever I want on the robot?

Could someone please explain me why the gyro should be in the centre of rotation. As far is I understand gyro measures angular velocity, which is exactly the same in every point of a rigid body.

I think the average acceleration would be the same in every point of the robot. When the robot starts leaning to some direction the upper part of the robot accelerates slowly and when it corrects the position the lower part accelerates rapidly.

Interesting question  ::)

I tend to agree with TJL for Gyros
Gyros sense rotation only, not acceleration.
Rotation is a "system wide" variable for a rigid system
They can be placed anywhere you want, even at the very top of the bot

I have a different analysis for accelerometers
Accelerometers measure both static acceleration (gravity) and dynamic acceleration (movement). They are also very noisy.
Static acceleration is system wide, dynamic acceleration (as speed) is not
As we are interested only in gravity measurement, we want dynamic  acceleration as low as possible.
The best place for reducing rotational acceleration is near the the motor axis.

Others may jump in and correct me if I am wrong
For those who build or already built a bot, where did you decide to place the sensors ??

@Patrik:   I went on your blog, nice start!
Keep feeding it and report here your baby's first steps   :)
Title: Re: Balancing robot for dummies
Post by: gibby623 on Oct 28, 2010, 01:29 pm
Dear Readers,

This is my understanding of the sensor's placement for a balancing system, the ideas are off the top of my head and should not be considered right unless you feel they are correct or can be proven. Feel free to correct me, I am writing my Thesis at the moment and any input would be encouraged. Without further ado, my answer.

Quote
I tend to agree with TJL for Gyros
Gyros sense rotation only, not acceleration.
Rotation is a "system wide" variable for a rigid system
They can be placed anywhere you want, even at the very top of the bot


I agree Kas. The basic calculation to vary between translational and rotational movement is v = wr, where the translational velocity (v) can be found by multiplying the angular velocity (w) by the radius it is rotating (r).

As the gyroscope a.k.a angular rate sensor is able to read only rotational velocities, it may be placed anywhere on the robot, assuming that the sensing axis is parallel to the axis of rotation of the mechanical system.

Quote
Accelerometers measure both static acceleration (gravity) and dynamic acceleration (movement). They are also very noisy.
Static acceleration is system wide, dynamic acceleration (as speed) is not
As we are interested only in gravity measurement, we want dynamic  acceleration as low as possible.
The best place for reducing rotational acceleration is near the the motor axis.


I partly agree with this. Accelerometers sense, as the name suggests, the acceleration along the corresponding sensing axis. However, I stray slightly from your definition Kas.

Your definition of dynamic acceleration seems more like the rate of change of acceleration, which, if anyone is interested it is known as a Jerk. I understand what you were trying to say though!

However, back to the point. In terms of a static system, the only acceleration the accelerometers will sense will the the earth's gravitational pull referenced to each sensing axis. This, for a balancing robot, is what you require from the sensor. However, the situation is complicated when the mechanical system also has translational acceleration. The Accelerometer sense translational accelerations, not angular acceleration. Returning back the to previous equation. As the rate of the angular velocity changes (i.e. angular acceleration) so does the translational acceleration of a particular point of the mechanical system, proportional to the radius of rotation.

So, what does this all mean? placing a gyroscope (theoretically) should be ok to place anywhere, paying close attention to the sensing axes.

Placing the accelerometer as close to the rotating axis is best. This can be proved considering if you placed the accelerometer precisely on the axis ie r = 0. Therefore the translational acceleration of the system will contribute wr = wx0 = 0m/s/s.

Does everyone agree? Too complicated? INCORRECT?

Regards!

P.S. Mentioned previously, I am writing my Thesis and also sitting exams therefore have had no time to program, however my more recent attempts of balancing can be seen here
-> http://www.youtube.com/watch?v=lRK9mlc0iD0 (moving forward for a given time stopping while balanced)
and
-> http://www.youtube.com/watch?v=wLFRkj4HIG0 (balancing, very unsteady!)

Regards, AGAIN!  ;D


Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Oct 28, 2010, 10:37 pm
Hi,  for months i just used the vertical angle , or its error from vertical to drive the PID data for the wheels. despite my best efforts the robot would drift and when pushed would catch up with vertical then stop the wheels but the upper body momentum would carry on giving a jerky start stop motion. so i tried adding in (or subtract ) the body velocity, calculated from the change in angle /dt , wheel velocity must be added to body velocity for a true body velocity. I had it all in different places but it was the same as N-bot's equation which I finally used in N-bots form.
Im not a SW engineer, code is messy.
I average the pwm, just by 2 but its jerky without. Code is for aDsPic.


setting Kpos=0, I tuned Kbody and Kbase which gives the lean back when pushed, also when moving between two locations the wheels move away ,changing the body angle towards the target, the wheels then change direction and the whole robot moves towards the target. when it reaches the target position the wheels speed up and overshoot to slow the body. Ill try and film this. The position error stops drift by offsetting the target vertical (which is incorrect if the robot drifts, and changes every time you breath), the robot will drift until the position offset gives a -ve motor drive when the robot will move back toward the zero position (not to the zero), but will oscillate around some offset.
As for gyro positioning, the gyro is a mems device, A lump in a void on springs, accel will affect it although it is compensated out ??. yes both my sensors are in completely the wrong place

as you can see the Kalman filter is on commented out, I just don't have a spare lifetime to understand it (or tune it) and the comp filter works.

going to try cheap motors next with ball mouse encoders.  :) :)

Code: [Select]
// ***************** gyro read and average *************
a_angle=BMA180_accel_read();

gyro_delta=gyro_read(gyro_null);
ga=ga+(gyro_delta*dt);

comp_angle = (0.997 * (comp_angle +(gyro_delta * dt)))+(0.003 * (a_angle));   // complimentary filter
angle[0]=(16*comp_angle)-vertical;  //  adjust for 1/16 degree , and vertical offset

//ars_predict(&filter_roll, gyro_delta, dt);    // Kalman predict

//K_angle = ars_update(&filter_roll, a_angle);        // Kalman update + result (angle)
   


//************ GET WHEEL VELOCITY************
velocity_read(dt,&M1_vel,&M2_vel,&map_x,&map_y,&heading,&delta_pos);  // returns velocity, updated x,y and current heading
base_vel=(M1_vel/2)+(M2_vel/2);

//************ GET BODY VELOCITY**********
body_vel=body_velocity(comp_angle,M1_vel,M2_vel,dt,height);

// **********GET POSITION *************
// use a local x which is referenced to the wheels only and not any map.use this to correct for drift
// and to calibrate the balance vertical this local X can be negative where distance to goal cannot
//get_heading(map_x,map_y,req_x,req_y,&req_heading,&distance);

//if (mode==0)req_heading=0;

heading_error=heading-req_heading;



local_x=local_x+delta_pos; //use x,heading should already be correct
pid_pos_error=position_PID(local_x,Pp,Pi,Pd,dt);


//if ((heading_error>5)||(heading_error<-5)) distance=0;   //  get correct heading before

//pid_pos_error=-position_PID(distance,Pp,Pi,Pd,dt);  // for mode2 use distance

//******************* Heading PID ************************//


PID_heading=heading_PID(heading_error,Hp,Hi,Hd,dt);


// *** DO BALANCE PID **********************


c_angle=(angle[0]+angle[1]+angle[2]+angle[3])/4;
angle[3]=angle[2];
angle[2]=angle[1];
angle[1]=angle[0];


//error= sum of angle, body_velocity, wheel rate, position
error=((c_angle))+((Kbody*body_vel)/32)+((Kbase*base_vel)/32)+((Kpos*pid_pos_err
or)/16); // use 1/16 of a degree as standard    COMPLEMENTARY FILTER


balance_error=balance_PID(error,Kp,Ki,Kd,dt);   // get actual balance PID error

motor_PID(balance_error,PID_heading,Mp,Mi,Md,M1_vel,M2_vel,&M1_pwm,&M2_pwm,dt);


av3_pwm1=av2_pwm1;
av2_pwm1=av1_pwm1;
av1_pwm1=av0_pwm1;
av0_pwm1=M1_pwm;
M1_pwm=(av0_pwm1+av1_pwm1)/2;   //+av2_pwm1+av3_pwm1)/4;

av3_pwm2=av2_pwm2;
av2_pwm2=av1_pwm2;
av1_pwm2=av0_pwm2;
av0_pwm2=M2_pwm;
M2_pwm=(av0_pwm2+av1_pwm2)/2;  //+av2_pwm2+av3_pwm2)/4;





M1_dir=0;
if (M1_pwm >1) M1_dir=2;     // forward ?
if (M1_pwm <-1) M1_dir=1;     // forward ?

M2_dir=0;
if (M2_pwm >1) M2_dir=2;     // forward ?
if (M2_pwm <-1) M2_dir=1;     // forward ?

M1_pwm=abs(M1_pwm); // +deadzone;
M2_pwm=abs(M2_pwm); // +deadzone;
if (M1_pwm>200) M1_pwm=200;
if (M2_pwm>200) M2_pwm=200;

if (abs(error)>720) // stop motors if fallen over > 45 degrees
{      M1_pwm=0;
    M2_pwm=0;
}
//if (int_seconds<5)
if (motor_en & 1)
{
M1_dir=2;
M2_dir=2;
M1_pwm=0;  // test step resopnse
M2_pwm=0;  // test step resopnse
}

pwm_set(M1_dir,M1_pwm,M2_dir,M2_pwm);


Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Oct 29, 2010, 12:53 am
http://www.youtube.com/watch?v=H-cM0e-QhHU

as mentioned above , showing the effect of the full balance equation
angle+Vbody+Vbase+position,  when stepping a 256mm change in requested  (x) position every 8 seconds, 0, 0x100,repeated step.

has anyone tried graphing live data in excel, I have a one channel prototype , it lags a bit but its better than cut and paste.
Title: Re: Balancing robot for dummies
Post by: X-firm on Oct 29, 2010, 06:16 am
Thanks for all the info about the placement of the sensors on the robot. I think a have a idea now on how to build the robot..

I want to ask you guys also what you think should be the best settings for my accelerometer and may gyro.

LPR510AL Dual-Axis (Pitch and Roll or XY) Gyro with ±100°/s and 400°/s Ranges (http://www.pololu.com/catalog/product/1266)

MMA7341L 3-Axis Accelerometer ±3/11g with Voltage Regulator  (http://www.pololu.com/catalog/product/1252)      

3g or 11g
100 degrees/s or 400 degrees/s

My jig is now complete so I can start testing some things:
(http://iloapp.x-firm.com/data/_gallery//public/21/1288238940_resized.jpg?width=480&height=270)

You can see more info on my blog (http://www.x-firm.com/?cat=6)
Title: Re: Balancing robot for dummies
Post by: kas on Oct 29, 2010, 09:17 am
@Gibby623

Quote
The Accelerometer sense translational accelerations, not angular acceleration.
Hoops  :-[ I fully agree, the angular acceleration is obtained from two orthogonal accelererometers, using Arctg function.
Thanks for those very precise comments
What is the precise subject of your thesis ???

Looking at your video, your bot balances pretty well
You won't get much better without motor encoders

Care to share your code??
I will post the end of my tutorial and KasBot V1 code probably this week end

Title: Re: Balancing robot for dummies
Post by: X-firm on Oct 29, 2010, 01:42 pm
Ok I have one more question just to clarify something for me. The X-axis of the accelerometer should be in the rotational axis of the wheels. But should the axis from the gyro be a perpendicular axis to the X-axis of the accelerometer like an Y-axis?

Should i use the Gyro Y-axis on the IMU? I'm cute sure I should use the Y-axis...

I see to different things below, just to clarify this

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)



Quote

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
Title: Re: Balancing robot for dummies
Post by: kas on Oct 29, 2010, 02:16 pm
Quote
I want to ask you guys also what you think should be the best settings for my accelerometer and may gyro.
LPR510AL Dual-Axis (Pitch and Roll or XY) Gyro with ±100°/s and 400°/s Ranges
MMA7341L 3-Axis Accelerometer ±3/11g with Voltage Regulator      
3g or 11g
100 degrees/s or 400 degrees/s

I would definitly go for the shortest range (3g and 100°/s)
Mines are 3g 500°/s and I would appreciate more resolution for the gyro

Quote
Ok I have one more question just to clarify something for me. The X-axis of the accelerometer should be in the rotational axis of the wheels. But should the axis from the gyro be a perpendicular axis to the X-axis of the accelerometer like an Y-axis?
Should i use the Gyro Y-axis on the IMU? I'm cute sure I should use the Y-axis...
:-? :-?   Lets make it clear, you need:
- one gyro
- two accelerometers (could work with one)
The gyro should parallel to the rotational axis of the wheels
one Acc axis according to horizontal
one Acc axis according to vertical

Quote
I see to different things below, just to clarify this
Please elaborate
Title: Re: Balancing robot for dummies
Post by: kas on Oct 29, 2010, 02:23 pm
@beautifulsmall

Thanks for the information  :)
I will study the code and come back to you soon

Please post a link to the motors you are using and possibly a close up photo of your bot. Seems that this gearless setup should solve the backlash problem.
Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Oct 30, 2010, 02:51 am
Kas, the motors are the dog's B'llks, and cost me ,about £60 uk,each, after tax, ect.ie, not a commertial product, plenty of bal rob videos are with cheap motors so ive ordered some 120rpm, 6v from china, photo shows a bit of back axel so im hoping i can attatch a mouse encoder, ball.
on Earwig are RE-max 29, neobium magnet, coreless. DC motors, non-cogging,
I will explain the construction because i once took one apart. re-assembly was not in a clean room.
A commutated DC motor has a rotating core surrounded by a set of magnets , the coreless design has a solid magnet bar in the centre and the wire winding is spun into a tube which surrounds the magnetic core. 7-11 poles are used but the winding pattern effectivley gives an analoge effect , BUT they will vibrate at 1Khz, and anything less, they are super twitchy!very low inertia.
Backlash, belt systems are good, belt tension matters, on earwig i have M3 HTD belts and 1:8 ratio, aluminium motor pulley, plastic wheel pulley. The motors are 20N side load max, the belts are 100N max, so I do run with a loose belt, and backlash.
Backlash compensation,  in wheel velocity calculation :

if velocity changes direction , velocity =0 for next two loops

because im rubbish at code, I set a loop to count time and let it loose every 4ms , before this the loop ran at its natural speed and despite having a dt in all the calculations, I needed to reset all the pid values constantly.
the velocity at direction change should be around zero or at least trying to get there so setting wheel velocity to zero cuts out the velocity jump as the motor changes direction and the belt tension changes from one side to the other. this does not affect encoder absolute position which must be read at static zero up.

blackfin , stereo vision,
Kas, its so hard to teach kids there 0xff times table :D ;D
Title: Re: Balancing robot for dummies
Post by: kas on Oct 30, 2010, 09:34 am
Quote
Reply #46 - Today at 02:51:56
beautifulsmall, on which side of the Atlantic ocean are you living ??
Should you live on the "wrong side"  ;), I hope you have another life for sleeping  :D  :D

Thanks for you coreless motor description, I was confusing "coreless" and "brushless" motors (the ones with separate ESC controllers)

Quote
ive ordered some 120rpm, 6v from china
Depending on wheels size and bot weight, you may find 120rpm a bit slow for proper operation

Planetary geared motor are supposed to have no mecanical backlash
Still trying to identify adequate units with attached encoders
Title: Re: Balancing robot for dummies
Post by: X-firm on Oct 30, 2010, 11:33 am
Finally after a lot of stupid questions I got the kalman filter to work. I need to tune the zero ACC_Z value some more and some other things but it works. The effect is quite amazing,  see the video here and see for yourself. (Values in quids). (http://www.youtube.com/watch?v=mYjOmtv90J0)

I was to say the least confused about the unit quids and the arrangement of the acc. and the gyro. But now I understands it and man it's smart to be able to us int values...


I will post some more information about the tests on my blog later this weekend... (http://www.x-firm.com/?cat=6)
Title: Re: Balancing robot for dummies
Post by: kas on Oct 30, 2010, 08:14 pm
Very clean signal   8-) 8-)
Also check signal quality when sensor is upside down
My sensor combo gives poor result when inversed and couldn't be positioned as per your photo

For ACC_Z, gravity correction = (0° value - 180° value)/2

Quids are nice for integer math
Those are basic 10 bits Quids (360° = 1204 Quids)
For better resolution, we may use Quids12 (360° = 4096 Quids)  ;)
Title: Re: Balancing robot for dummies
Post by: kas on Oct 30, 2010, 08:31 pm
@patrick
If your are still looking for a 12V battery, look at this one
http://cgi.ebay.co.uk/ws/eBayISAPI.dll?ViewItem&item=150447448378
3800 mAh, 22 bucks shipped
Title: Re: Balancing robot for dummies
Post by: X-firm on Oct 30, 2010, 08:36 pm
@Kas

I have some small things I wanted to ask you that I don't really understand. I used the greyed sensor inputs like in the image below is that correct:
(http://iloapp.x-firm.com/data/_gallery//public/21/1288460529_resized.jpg?width=400&height=489)

I changed the following in your code:
I recalculated the zero value for Acc_Z
440/3300*1024 = 136
Code: [Select]

void calibrateSensors() {    // Set zero sensor values
 long v7
 for(int n=0; n<5; n++) {
   v = 0;
   for(int i=0; i<50; i++)       v += readSensor(n);
   sensorZero[n] = v/50;
 }                                                          
 sensorZero[ACC_Z] -= 137;                        
}


I changed the gyro rate and also changed to my GYR_X instead of using GYR_Y. I also changed the sign off the return value;
Code: [Select]

int getGyroRate() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
 return int((sensorValue[GYR_X] * 0.88888888889)*-1);                 // in quid/sec:(1024/360)/1024 * 3.3/0.010)
}

int getAccAngle() {                      
 return (arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256);    // in Quid: 1024/(2*PI))
}


What I don't really understand is why I didn't need to change the sign of the ACC_X value to get it like your values.

Why do you enter the values as negative in to the function arctan2 and why adding 256?
Quote

(arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256)


I'm not that good at math so you have to excuse my sometimes stupid questions..

Title: Re: Balancing robot for dummies
Post by: X-firm on Oct 30, 2010, 10:46 pm
@kas
Thanks for the tips.

I had planed to by a lipo battery from Hobbyking but they are on 11,7V I think so this suited me just fine and free shipping also :)

Quote

@patrick
If your are still looking for a 12V battery, look at this one
http://cgi.ebay.co.uk/ws/eBayISAPI.dll?ViewItem&item=150447448378
3800 mAh, 22 bucks shipped
Title: Re: Balancing robot for dummies
Post by: kas on Oct 31, 2010, 09:05 am
Quote
I used the greyed sensor inputs like in the image below is that correct:
Nice drawing  ;), my setup is actually as per your drawing


Quote
I recalculated the zero value for Acc_Z
440/3300*1024 = 136
I looked at the MMA7341L Accelerometer datasheet:

(http://imgur.com/a127C.jpg)

Please keep in mind that quoted sensitivity (440 mV/g) is a typical value. Your specific sensor may be as low as 413, or as high as 466.
I suggest you calculate Acc_Z zero value using the experimental way as described above


Quote
What I don't really understand is why I didn't need to change the sign of the ACC_X  value to get it like your values.
Why do you enter the values as negative in to the function arctan2 and why adding 256?
Quote:

(arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256)
Signs are related to axis orientation.
Just adjust the signs by experimenting and make sure you get data format as described in reply #7.
256 is an offset (90°) to get a 0 angle value when the bot is vertical
Title: Re: Balancing robot for dummies
Post by: X-firm on Oct 31, 2010, 05:03 pm
@kas

Tanks for the replay of all my questions and I bought the battery you gave me a link to.

I will make some more experiments with the kalman filter and also add the other axis so I can see if the bot is tilting side wise as well.

Under the week I will start moving the motors and make some experiments with the encoders.

In the weekend I will start building the body to the robot...
Title: Re: Balancing robot for dummies
Post by: kas on Oct 31, 2010, 07:11 pm
you may experiment with the encoders, but I strongly advise not to implemente them in the bot yet.
You can balance pretty well w/o encoders
Encoders will reduce potential forward/backward drifting

Good luck  ;)
Title: Re: Balancing robot for dummies
Post by: kas on Oct 31, 2010, 07:20 pm
                          [size=14] ** Balancing robot for dummies **[/size]


 [size=14]Part five (final): DC motor control and V1 code[/size]

The updatePid function (see part 4) returns a -255 +255 range value for the motors commands
    0        stop (bot is vertical)
+255      fast forward
-255       fast backward

Motor controllers only understand 0 +255 range values
This is taken care of in the Drive_Motor code

Code: [Select]
int Drive_Motor(int torque)  {
 if (torque >= 0)  {                                        // drive motors forward
   digitalWrite(InA_R, LOW);                        
   digitalWrite(InB_R, HIGH);
   digitalWrite(InA_L, LOW);                    
   digitalWrite(InB_L, HIGH);
 }  else {                                                  // drive motors backward
   digitalWrite(InA_R, HIGH);                      
   digitalWrite(InB_R, LOW);
   digitalWrite(InA_L, HIGH);                      
   digitalWrite(InB_L, LOW);
   torque = abs(torque);
 }
 analogWrite(PWM_R,torque);
 analogWrite(PWM_L,torque * 0.9);                           // motors are not built equal...

Please refer to http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0
for additional informations

The first part of this tutorial is now terminated (finally...  :-X :-X :-X)
I am impressed by the number of time this thread has been viewed, I hope the info was clear and detailed enough
If you started your own project or intend to do so in the near future, please jump in now and just leave a comment here


KasBot V1 complete code file:
** EDIT **  can't find the trick to upload a file   any hint ???

KasBot V1 is fully functional and should balance for ever
Just modify the code according to your own gyro sensitivity
Code: [Select]
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)  
}
My gyro (IDG300) is 2mV/deg/sec
For Accelerators, adjust gravity offset in calibrateSensors()
PID parameters should also be tuned according to your own bot geometry and weight

More to come:
V1.5 will bring "utilities" for live tuning without re uploading the code
V1.7 will replace Arduino 10 bit ADC with MPC3208 12 bit ADC (increased resolution)
V2.0 should combine encoders info in PID to suppress potential forward/backward drifting
V3.0 should bring RC remote control features (Xbee technology)
V4.0 will add IR Ping))) sensor for obstacle avoidance
V5.0 will add...    your own ideas  ;) ;)

Long (probably endless) roadmap  ::)
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Oct 31, 2010, 08:02 pm
I have to say this is awesome! You managed to write it for dummies, I hope I'm dumb enough to get it, hehe. I have different acc and gyro sensors, old ones, I have to dig up some data sheets to find out the resolution for them. Other than that, I should be fine, except for motors speed, that is lower than 200 rpm. Oh well, I'll use them for now. I'll add some 10k pots to adjust the PID parameters. I'll report my progress probably next weekend, during the week I don't have much time. This weekend will be the chassis manufacturing out of some plywood. After I get to the final tweaks I'll cut a nicer one from acrylic. Wish me luck!
Title: Re: Balancing robot for dummies
Post by: kas on Oct 31, 2010, 08:47 pm
Hi Ro-Bot-X
Thanks for the nice comments

Quote
...Wish me luck!
I do!!!   :) :) :)
Title: Re: Balancing robot for dummies
Post by: hoyeungl on Nov 01, 2010, 03:12 pm
I need the infomation about the PID which you stated.

How come in your error terms is a speed, and the return part is a pwm duty ratio?

What is the K stands for? Gain? or others?
How can I calculate it?
Title: Re: Balancing robot for dummies
Post by: kas on Nov 01, 2010, 08:01 pm
:( :( :(

"Hi"

carefully ready the two threads and come back if you didn't find your answers

"Thank you"
Title: Re: Balancing robot for dummies
Post by: hoyeungl on Nov 02, 2010, 07:03 am
Quote
"Hi"

carefully ready the two threads and come back if you didn't find your answers

"Thank you"


I have read two posts almost 3 times....  
In post 1 , I found that this code don't have the Ki term.
Code: [Select]
int updatePid(int command, int targetValue, int currentValue)   {             // compute PWM value
float pidTerm = 0;                                                            // PID correction
int error=0;                                  
static int last_error=0;                            
 error = abs(targetValue) - abs(currentValue);
 pidTerm = (Kp * error) + (Kd * (error - last_error));                            
 last_error = error;
 return constrain(command + int(pidTerm), 0, 255);
}

After I search for two posts I found something useful in post 2 =>Part four: PID control
But while I study your code
Code: [Select]
int updatePid(int targetPosition, int currentPosition)   {
 int error = targetPosition - currentPosition;
 pTerm = Kp * error;
 integrated_error += error;                                      
 iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
 dTerm = Kd * (error - last_error);                            
 last_error = error;
 return -constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}

I got confuse why the first post's code "updatePID" you got three input variables but second one you got 2 inputs:int targetPosition, int currentPosition.
Maybe I am not familiar with PID, would you please tell me that what is the value for -GUARD_GAIN and GUARD_GAIN?
Also, What is the K means?
Code: [Select]
-constrain(K*(pTerm + iTerm + dTerm), -255, 255);
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 02, 2010, 01:34 pm
I'm working on GUI in processing where you will see all important parameters and sensor inputs. And be able to change parameters in the arduino on the fly and save all settings in the arduino memory.

Also in the future be able to control the bot wireless true a blueSmirf.

Quote
V1.5 will bring "utilities" for live tuning without re uploading the code


Are you also working on something? Interested of co-op?  ;)
Title: Re: Balancing robot for dummies
Post by: kas on Nov 02, 2010, 08:04 pm
Quote
In post 1 , I found that this code don't have the Ki term.
Code: [Select]
int updatePid(int command, int targetValue, int currentValue)   {             // compute PWM value
float pidTerm = 0;                                                            // PID correction
int error=0;
static int last_error=0;
 error = abs(targetValue) - abs(currentValue);
 pidTerm = (Kp * error) + (Kd * (error - last_error));
 last_error = error;
 return constrain(command + int(pidTerm), 0, 255);
}
The first code snippet was created as a stand alone PID motor control application.
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0
It allows to run a DC motor at very low speed while retaining normal torque.
For this type of application, Integral parameter is generally not required.

Quote
After I search for two posts I found something useful in post 2 =>Part four: PID control
But while I study your code Code:
Code: [Select]
int updatePid(int targetPosition, int currentPosition)   {
 int error = targetPosition - currentPosition;
 pTerm = Kp * error;
 integrated_error += error;
 iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
 dTerm = Kd * (error - last_error);
 last_error = error;
 return -constrain(K*(pTerm + iTerm + dTerm), -255, 255);}

I got confuse why the first post's code "updatePID" you got three input variables but second one you got 2 inputs:int targetPosition, int

currentPosition.
For a balancing robot targetPosition should always be "zero"
I chose to create this variable to allow fine tuning and later for RC control

Code: [Select]
PWM_val= updatePid(PWM_val, speed_req, speed_act);                        // compute PWM valuePWM_val parameter is not actually modified in updatePid and my original code was:

Code: [Select]
 PWM_val= PWM_val + updatePid(speed_req, speed_act);                        // compute PWM value
 PWM_val = constrain(PWM_val + int(pidTerm), 0, 255);
I decided to pass PWM_val as a parameter to allow constrain(XXX, 0, 255) to sit in the updatePid function.
Both implementations are equivalent.

Quote
Maybe I am not familiar with PID, would you please tell me that what is the value for -GUARD_GAIN and GUARD_GAIN?
Also, What is the K means?
Integral is the only term that keeps changing no matter how small the error remains
GUARD_GAIN sets up a limit for iTerm. It is standard procedure to clip Integral.
K, Kp, Ki and Kd are parameters to be adjusted purely on experimentation
Title: Re: Balancing robot for dummies
Post by: kas on Nov 02, 2010, 08:44 pm
@Patrik
I am not really familiar with Java/Processing,  :-[
Building a GUI is a great idea, please post ASAP

I just finished a much simpler version, using the IDE serial monitor:


Code: [Select]
int getTuning() {
 if(!Serial.available())    return 0;
 delay(10);                  
 char param = Serial.read();                            // get parameter byte
 if(!Serial.available())    return 0;
 char cmd = Serial.read();                              // get command byte
 Serial.flush();
 switch (param) {
   case 'p':
     if(cmd=='+')    Kp++;
     if(cmd=='-')    Kp--;
     break;
   case 'i':
     if(cmd=='+')    Ki++;
     if(cmd=='-')    Ki--;
     break;
   case 'd':
     if(cmd=='+')    Kd++;
     if(cmd=='-')    Kd--;
     break;
   case 'k':
     if(cmd=='+')    K +=0.2;
     if(cmd=='-')    K -=0.2;
     break;
   default:
     Serial.print("?");           Serial.print(param);
     Serial.print(" ?");          Serial.println(cmd);
   }
 Serial.println();
 Serial.print("K:");           Serial.print(K);
 Serial.print("   Kp:");       Serial.print(Kp);
 Serial.print("   Ki:");       Serial.print(Ki);
 Serial.print("   Kd:");       Serial.println(Kd);
}

Usage:
to increase Kp: type "p", then "+" then "enter"
to decrease Kp: type "p", then "-" then "enter"
same procedure for Ki: "i" , for Kd: "d" and for K: "k"
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 03, 2010, 06:47 am
@kas
Quote

I am not really familiar with Java/Processing,  
Building a GUI is a great idea, please post ASAP


I have started with the GUI and I want it widget based some please tell me what type of widget you want to have in the GUI.

You can see more info about it on my blog. (http://www.x-firm.com/?p=132)

It's only a beta, I will improve the graph widget and clean up the code a little then I will post the source on my blog and put a link here.

http://www.youtube.com/watch?v=VwivE1H1NZs
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 05, 2010, 09:04 pm
Heres the first beta version of the Balancing Bot GUI and it includes one widget that are a dynamic graph...
     

  • You can add as many graphs as you want and set the size and shape of them
  • You can maximiz a graph
  • You can also move it in real time

Right now you can only show information from the bot, but I will soon update the GUI with the possibility to change parameters in the bot.

Below is a image of the GUI that are using three Graph widgets:

(http://iloapp.x-firm.com/data/_gallery//public/21/1288984968_resized.jpg?width=480&amp;height=360)

(http://iloapp.x-firm.com/data/_gallery//public/21/1288984977_resized.jpg?width=480&amp;height=360)


Heres the code from the example above...
(http://www.x-firm.com/processing/Blog/BalancingBotControl.pde)

Sketch for Processing IDE:

Download and unzip the folder in your sketch directory. Change the serial port to the corresponding for you computer...

Complete sketch  (http://www.x-firm.com/processing/Blog/BalancingBotControl.zip)

Title: Re: Balancing robot for dummies
Post by: kas on Nov 06, 2010, 10:09 am
Hi Patrik

I went on your blog http://www.x-firm.com/?cat=6
nice job, I will have to jump in Processing some days  ::)


Quote
I have started with the GUI and I want it widget based some please tell me what type of widget you want to have in the GUI.

Those are the parameter I did have to adjust or monitor:

- STD_LOOP_TIME           adjustable
- lastLoopUsefulTime        read only
- lastLoopTime                read only
- ACC_X, _Y, GYR_Y        read only or trending
- actAngle, ACC_angle     trending
- setPoint                        adjustable
- K, P, I, D                      adjustable
- pTerm, iTerm, dTerm     trending
- drive                            trending

More to come with encoder and RC implementations ;)


Code: [Select]
Heres the code from the example above...

Sketch for Processing IDE:

Download and unzip the folder in your sketch directory. Change the serial port to the corresponding for you computer...

Complete sketch
Your links are dead  :(
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 06, 2010, 07:26 pm
@kas

I will make so we can change the parameters as you describable below..
Quote

- STD_LOOP_TIME           adjustable
- lastLoopUsefulTime        read only
- lastLoopTime                read only
- ACC_X, _Y, GYR_Y        read only or trending
- actAngle, ACC_angle     trending
- setPoint                        adjustable
- K, P, I, D                      adjustable
- pTerm, iTerm, dTerm     trending
- drive                            trending


I have fixed the links..
Quote

Heres the code from the example above...
(http://www.x-firm.com/processing/Blog/BalancingBotControl.pde)

Sketch for Processing IDE:

Download and unzip the folder in your sketch directory. Change the serial port to the corresponding for you computer...

Complete sketch  (http://www.x-firm.com/processing/Blog/BalancingBotControl.zip)
Title: Re: Balancing robot for dummies
Post by: kas on Nov 07, 2010, 06:44 am
Quote
Hi Kas,

first to tell you how much I like your post "balancing robot for dummies" and how useful it has been. I have been following it real time since you started it.

I was wondering if you solved how to upload the v1.0 of the code already or if you know where I can take a look to it if thats not to much asking!

keeep it up, thanks!

Thanks for the nice comments  8-)
Patrik has offered to host the code on his web site.
I sent him the zip by email, he will release the link pretty soon
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 07, 2010, 07:18 am
I have now uploaded the KasBot V1 code on my blog you can reach it from the link below:

http://www.x-firm.com/?p=142 (http://www.x-firm.com/?p=142)
Title: Re: Balancing robot for dummies
Post by: kas on Nov 07, 2010, 05:39 pm
               [size=18]KasBoT V1.0   is just above[/size][/color]

this thread is becoming a bit cluttered  ;), this bookmark should help for future reference.
Thanks Patrik for hosting the code on your blog
Title: Re: Balancing robot for dummies
Post by: T.J.L. on Nov 07, 2010, 05:58 pm
Thanks Patrik for the graph drawing sketch. I've been looking for a nice way to visualize the gyro & accelerometer measurement and now I have it :).

If you are going to develop the sketch further, I'd like to see some kind of easy scaling for the graph amplitude and also for update speed on x-direction. I did found a way to change them in code, but it could maybe be easier.
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 07, 2010, 10:00 pm
@T.J.L.
Quote

Thanks Patrik for the graph drawing sketch. I've been looking for a nice way to visualize the gyro & accelerometer measurement and now I have it .

If you are going to develop the sketch further, I'd like to see some kind of easy scaling for the graph amplitude and also for update speed on x-direction. I did found a way to change them in code, but it could maybe be easier.


It's nice to here that someone has had use of the GUI. And like u say the update speed is updated every time the Arduino sends anything. I can add some buttons for changing the scale..

I'm currently building my new robot and will adapt the KasBot_V1 code to my hardware to be able to help developing this further. When that are done the GUI developing will be more in focus...  
Title: Re: Balancing robot for dummies
Post by: pramlie on Nov 08, 2010, 05:44 am
The processing code not work for me. I just upload K_BotV1.pde and then run BalancingBotControl.pde on Processing. I got only 3 tables on the screen, without processing anything...If I'm doing wrong, would you tell me step-by-step procedure your GUI sketch Patrik? Thanks
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 08, 2010, 07:46 am
@pramlie

The BalancingBotControl.pde has not yet bean adapted to the K_BotV1 code so you need to change some small things in the Arduino Code and also in the BalancingBotControl.pde.

I can update the BalancingBotControl.pde under the day to be turn key finished for the K_BotV1. All come back when it's done...

You will den get the following functions:
- STD_LOOP_TIME           adjustable
- lastLoopUsefulTime        read only
- lastLoopTime                read only

- ACC_X, _Y, GYR_Y        read only or trending
- actAngle, ACC_angle     trending
- setPoint                        adjustable
- K, P, I, D                      adjustable

- pTerm, iTerm, dTerm     trending
- drive                            trending

There are no serial communication added in the K_BotV1 code, I will make a modified version that has the serial part added that are adapted for the GUI..
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 08, 2010, 12:25 pm
I have added serial communication to the KasBotV1 code and called it KasBotV1_Serial. The serial communication is aimed for the Balancing Bot GUI.

You can now download dose files in the same blog post as the KasBot_V1 code without serial communication. And also the Balancing Bot GUI

http://www.x-firm.com/?page_id=145
Title: Re: Balancing robot for dummies
Post by: kas on Nov 08, 2010, 08:21 pm
Just received from Sparkfun:

(http://imgur.com/gbSR5.jpg)
The basic kit for RC and wireless debuging

2X  1mW Xbee modules  http://www.sparkfun.com/commerce/product_info.php?products_id=8664
1X  Xbee wireless shield  http://www.sparkfun.com/commerce/product_info.php?products_id=9841
1X  Xbee explorer regulated  http://www.sparkfun.com/commerce/product_info.php?products_id=9132
1X  Xbee Explorer USB dongle  http://www.sparkfun.com/commerce/product_info.php?products_id=9819
More info to come soon  8-)
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 09, 2010, 06:47 am
@kas

I have one question about the encoder:

How did you wire them ? Did you use any pulldown resistors?

Nice parts I haven't decided if I'm going to use the bluesmirf or Xbee but it will be nice getting a report about the Xbee from you.. :)

The 1X  Xbee explorer regulated is not need if I just want to communicate with a Arduino UNO?
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Nov 09, 2010, 01:28 pm
Hi Patrick,

I have used the xBees for a wireless serial link and also for wireless code uploading. From what you see in the above picture, you will need:
- one xBee installed on the xBee Explorer - this will be connected to the computer
- one xBee installed in the Shield - this will be installed on the Arduino Uno
- one xBee installed on the regulated board - this will be used to communicate with other microcontroller boards instead of the Shield

You can also install the Shield on your Uno and the regulated board on a different microcontroller and have them communicate wirelessly. This kit allows you to do what ever combinations you might need.

I have 2 regulated boards. I connect one to my microcontroller board on the FTDI connector and the other to the FTDI cable through an inverter connector that swaps Rx with Tx. (The Explorer already does this and has the FTDI onboard.)

Now that I have my webstore (http://robotxdesigns.ca) open, I will start working on the Compact robot to make it balance. I intend to use the xBees and your Processing GUI to adjust the parameters on the fly. I hope it works!
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 09, 2010, 02:04 pm
Nice site, I have to look closer on your boards and kits.

Good job:)  
Title: Re: Balancing robot for dummies
Post by: kas on Nov 09, 2010, 07:29 pm
Hi Ro-Bot-X

I really like the design of your website, clean and appealing.  :)
A lot of nice stuff also. I have to go back and spend some more time there  ;)

Quote
Now that I have my webstore open, I will start working on the Compact robot to make it balance.
One cosmetic question:
What is the trick to embed a link (http://robotxdesigns.ca/) in a text (webstore)??
Title: Re: Balancing robot for dummies
Post by: kas on Nov 09, 2010, 07:57 pm
Xbee's are beauties ::)
They have 2 operating modes:
- Transparent Mode. When operating in this mode, the modules act as a serial line replacement.
- API mode. This mode allows more sophisticated operations,including communication with the analog and digital I/O's embeded in the Xbee chip.  

If you take the Xbee road, go for the Serie 1 they work right out of the box and there is tons of tutorials.
Series 2 allows mesh networking, but do not allow direct Xbee to Xbee communication w/o microcontroller (look here (http://www.adafruit.com/blog/2009/04/21/using-xbees-to-control-relays/)) .
Caution! Series 1 and Series 2 are not compatible, they don't talk to each other

Transparent mode is perfect for remote debugging using BalancingBotControl

More infos:
- Data sheet (http://www.sparkfun.com/datasheets/Wireless/Zigbee/XBee-Manual.pdf)  
- Configuration software (http://www.digi.com/support/kbase/kbaseresultdetl.jsp?kb=125)  
- Madame Ada super tutorial (http://www.ladyada.net/make/xbee/arduino.html)  

edit: much cleaner now, thanks Ro-Bot-X for the tip below  ;)
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Nov 10, 2010, 03:33 am
Thanks Kas,
The webstore is just a wordpress blog with a simple shopping cart that goes to Paypal on checkout. I have a nice Open Cart store installed but I didn't have time to add the products and everything else it needs. Perhaps later, when I'll have more products.

BTW, you can see Compact here (http://letsmakerobots.com/node/23022).

Here is how to embed a link:
- write the word(s) you want to represent the link
- highlight it(them)
- click on the Link button
- click inside the [url] wrapper and write it like this:

I got a ton of work this week so I will have to let robot experimenting over to the weekend. Sigh...
(http://http//...)
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 10, 2010, 08:09 am
I found this clip on youtube and I thought it was a nice way of building a robot to us with this project and also others..

(http://iloapp.x-firm.com/data/_gallery//public/21/1289372898_resized.jpg)


http://www.youtube.com/watch?v=RcNoL3vtXFs
Title: Re: Balancing robot for dummies
Post by: kas on Nov 10, 2010, 09:31 am
Very impressive :o :o
3 years ago when I first thought about building a balancing robot, I saw a very similar one:  "Botka, the Barely Standing Robot", based on a GyroMax R/C car frame

(http://imgur.com/MzPCr.jpg) (http://imgur.com/MzPCr.jpg)
See the video (http://play.killerfrog.com/l0k1e8k1a6/Botka-The-Barely-Standing-Robot.html)
No information seems to be available for these 2 projects  :( :(
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 10, 2010, 02:45 pm
I have now stared building my robot and I wanted to use parts so I had to machining as little as possible.

I thought of using 4 standard 6mm threaded rod and 3 nail ink and some nuts and washers as in the image below.

(http://iloapp.x-firm.com/data/_gallery//public/21/1289396136_resized.jpg?width=480&height=574)
This should be parts that every one can find in there local hardware stores..

I'm a little concerned of the heavy weight of this structure but it's a little below 1 kg. It will be cleared after a test run.

(http://iloapp.x-firm.com/data/_gallery//public/21/1289396143_resized.jpg?width=250&height=333)

What do you think about the robot chassis? :o
Title: Re: Balancing robot for dummies
Post by: kas on Nov 10, 2010, 09:35 pm
Nice frame, built as a tank  :) :)
For info my bot is just over 1kg, including motors and electronic, w/o battery
Total height: 32 cm (12.5")

Quote
@kas
I have one question about the encoder:
How did you wire them ? Did you use any pulldown resistors?
For the moment I have connected only one encoder
I use internal pullUp resistors, this is the relevant code snippet:

Code: [Select]
void setupEncoder()  {
 pinMode(encodPinA1, INPUT);
 pinMode(encodPinB1, INPUT);
 digitalWrite(encodPinA1, HIGH);              // turn on pullup resistor
 digitalWrite(encodPinB1, HIGH);
 attachInterrupt(1, rencoder, FALLING);
}
Title: Re: Balancing robot for dummies
Post by: jondaeh on Nov 11, 2010, 09:27 pm
Hi!

Great thread kas, a lot of useful information as I'm making a balancerobot for a school project:)!

I have also been (trying to) making a quadrocopter for a year or so, and both projects have stopped at the same problem. Gibby623 posted a reply at the first page of this thread, which seems like being a similar problem:

Quote
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?  


I have been using a IMU 5dof from Sparkfun on both projects, and a complimentary filter(now gonna be a kalman after reading this thread:D) for fusing the accelrometer and the gyro.

I'm using the non-amplified output from the IMU(sens. 2 mv/deg/sec). The reading from the gyro is scaled to millivolts in analogRead(see remark at bottom).

"sampletime" is in millisecs, and stored as a integer.
"gyroRate" and "xAngle" is doubles.

The angle is then integrated with the following algorithm:

Code: [Select]

gyroRate = (double)(analogRead(gyroPin) - gyroOffset);
xAngle += (gyroRate / 2) * (sampletime/1000) ;


I'm then measuring the angle of the robot manually, and when I apply 45 degrees from equilibrium, the integrated angle is about 15-20 degrees.  

I have also tried finding the mean value of the two last gyro rates:

Code: [Select]

thisGyroRate = (double)(analogRead(gyroPin)-gyroOffset);
gyroRate = (lastGyroRate + thisGyroRate) / 2;
lastGyroRate = thisGyroRate;
xAngle += (gyroRate/2)*(sampleTime/1000);


But it didn't make much difference.

It seems to me that the IMU outputs "to little rate". If I cheat and "change" the sensitivity to about three times less, the output seems about right. But this can't be the way to go;)?

Off course, I may have got the whole integrations thing wrong. A basically keep adding the (gyrorate/sensitivity)*dt to a variable of type double.. Right?

Gibby623 @ page #2 in this thread:
Quote
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...


Well, my gyro drifts like slicks on snow too.. Maybe about 0.5 degree pr second. However I don't now what's a normal drift, and as the complimentary filter handles the drift pretty well I guess kalman will make it just better:)

To sum(integrate, I loathe by my humor:P) up:

To Gibby623:

What was the solution to your sensitivity problem?

And to everyone else:
Have anyone experienced a similar problem?


Again, keep up the good work kas! This is a interesting and informative thread:D!

Jon, Norway.



Remark:
I'm actually using a Fez Domino, not an Arduino. The Domino automatically scales the values read from the analogport to millivolts(0-3300).  
The reason I'm still posting this in a Arduino forum is this is the only place I have found anyone with a problem similar to mine.


Title: Re: Balancing robot for dummies
Post by: uncle_tom on Nov 12, 2010, 09:52 pm
Hi guys, I've been following this thread for a while. Any news on how to add remote control, with say PC USB Gamepad and Xbee. That would add icing to the cake.

//uncle_tom
Title: Re: Balancing robot for dummies
Post by: kas on Nov 12, 2010, 10:56 pm
[size=14]Announcement:

The complete Balancing robot tutorial is now hosted on Patrik's blog (http://www.x-firm.com/?page_id=148)
It will be updated on a regular basis.[/size]
[/color]

This thread will stay alive for some time as a "support forum".
The thread has now been read over 10,000 times
Thanks for your ongoing (but rather silent  ;)) interest

Title: Re: Balancing robot for dummies
Post by: kas on Nov 12, 2010, 11:02 pm
Quote
I'm then measuring the angle of the robot manually, and when I apply 45 degrees from equilibrium, the integrated angle is about 15-20 degrees.


Hi Jon D, wellcome aboard  :)
I suspect you have a scaling problem.
You may also be trying to mix oranges and bananas (degrees and radians).
Finally make sure your gravity compensation for the "vertical" accelerometer is OK

Back to the basic, go there: http://www.x-firm.com/?page_id=189
Print your raw data (ACC_a, ACC_b, GYR) using the serial monitor

(http://imgur.com/qvCb3.jpg)
Can you reproduce this data layout ??
Is your gyro sign correct ??

Hopefully, Gibby623 will jump in and share his experience.
Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Nov 13, 2010, 12:57 am
10K hits and still no commertial product, in the last 12 months of work I have learned its still very difficult to get a stable balance. but what then, mapping, prediction and recognition. we have had 4-wheeled non balancing robots for years but only hoovers .Mapping is the next hurdle.
Title: Re: Balancing robot for dummies
Post by: kas on Nov 13, 2010, 04:18 pm
Quote
Hi guys, I've been following this thread for a while. Any news on how to add remote control, with say PC USB Gamepad and Xbee. That would add icing to the cake.

//uncle_tom

Hi uncle_tom
Are you looking for that ???   ;) ;)
(http://imgur.com/AhMsl.jpg)



Quote
10K hits and still no commertial product, in the last 12 months of work I have learned its still very difficult to get a stable balance. but what then, mapping, prediction and recognition. we have had 4-wheeled non balancing robots for years but only hoovers .Mapping is the next hurdle.

As you can see above, the project is moving
The road is long but fascinating and really educational

Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Nov 13, 2010, 10:47 pm
Hey Kas,

I finally got time to work on my robot. My old sensors are 5V (they support 3.3V but it's easier for me to use 5V) and have different sensitivity.

The Accelerometer is a 5g ADXL320, 312mV/g at 5V, so I have calculated the "zero" to 63. The X axis is horizontal with positive towards the front of the robot. The Z axis is vertical with the positive towards the top of the robot.

The Gyro is a 300deg/sec ADXRS300, 5mV/deg/sec, so I have calculated the "rate" to 2.777777 . The Gyro axis is parallel with the wheel axle with the positive rotating towards the front of the robot.

I have problems with the signs of the Acc axes.  For instance, I have
for 0degrees (bot standing upright): AccX=0, AccZ=63
for +90 degrees (bot tilted towards front): AccX=63, AccZ=130
for -90 degrees (bot tilted towards back): AccX=-67, AccZ=125
for 180 degrees (bot upside down): AccX=0, AccZ=193

What do I need to do (in software!) to make it right?

Thanks!
Title: Re: Balancing robot for dummies
Post by: kas on Nov 14, 2010, 08:02 am
Quote
I have problems with the signs of the Acc axes.  For instance, I have

for 0degrees (bot standing upright): AccX=0, AccZ=63
for +90 degrees (bot tilted towards front): AccX=63, AccZ=130
for -90 degrees (bot tilted towards back): AccX=-67, AccZ=125
for 180 degrees (bot upside down): AccX=0, AccZ=193



Hi Ro-Bot-X

Code: [Select]
 sensorZero[ACC_Z] -= 100;            // Sensor: horizontal, upward
Gravity correction sign is wrong
You added 63, should be substracted.
You will then obtain:

0 degrees:       AccX=0,       AccZ=-63
+90 degrees:   AccX=63,     AccZ=4
-90 degrees:    AccX=-67,    AccZ=-1
180 degrees:    AccX=0,      AccZ=67

65 would probably be a better value for gravity correction
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 14, 2010, 08:16 am
Quote
Announcement:

The complete Balancing robot tutorial is now hosted on Patrik's blog
It will be updated on a regular basis.


[size=24]I saw that it was the wrong link in the announcement...
This is the right link:[/size]
http://www.x-firm.com/?page_id=145
Title: Re: Balancing robot for dummies
Post by: kas on Nov 14, 2010, 08:27 am
Thanks Patrik   ;)
Title: Re: Balancing robot for dummies
Post by: rwehner186 on Nov 15, 2010, 09:21 pm
Has anyone tried this project with stepper motors? Would this make the task easier or more complicated?
Title: Re: Balancing robot for dummies
Post by: kas on Nov 17, 2010, 07:41 am
@Ro-Bot-X

Quote
I have used the xBees for a wireless serial link and also for wireless code uploading.

I am currently working on wireless debugging using Xbee and Patrik's Balancing Bot GUI.
The two ways communication between bot and PC is OK, but I didn't succeed wireless code uploading sofar.
I understand additional components are needed to force reset, please elaborate.

Is your bot balancing now ??



@RWehner

Quote
Has anyone tried this project with stepper motors? Would this make the task easier or more complicated?

I have seen lots of balancing bot using continuous rotation servo motor or DC motors (with or w/o encoders). I have yet to see a working implementation based on stepper motors.
I am not to familiar with stepper motors, but I clearly see the interest of your question. Others will certainly jump in with additional info.


Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 17, 2010, 09:56 am
I have now uploaded a new version of the GUI and bot code where you now can change the values of the following parameters:

setPoint
K
Kp
Ki
Kd

http://www.x-firm.com/?page_id=145

I'm working on the 1.3 version where I have cleaned up the code a lot and also added the possibility to change the STD_LOOP_TIME. And also see the values of the lastLoopUsefulTime, lastLoopTime and error..  I will also have added a setting for screen size so the controls should adapt to the screen size so the GUI should look the same regardless which screen size you have..

@kas
Are you happy with the xbee kit? How are you implementing it on the kas bot? Do you have the shield on the bot and the USB explorer card in the PC?
Title: Re: Balancing robot for dummies
Post by: uncle_tom on Nov 17, 2010, 05:15 pm
Great progress on the balancing part, but is anyone working on remote control for this Arduino robot? I built several balancing robots using NXT and VEX kits and this is a great learning project, but I hesitate to spend $400 until I see that someone has implemented remote control of some kind. I think that just the robot without RC is not complete.

keep it up guys, you are doing a good job!
Title: Re: Balancing robot for dummies
Post by: kas on Nov 17, 2010, 08:08 pm
Quote
How are you Kas?

I just wanted to thank you again for your post *balancing robot for dummies* it helped me a lot, and  want you to be the first one to see my improvements:
http://www.youtube.com/watch?v=oM-j-L3Jg7Q
this is using a complementary filter and PID with a sparkfun IMU... going to use encoders soon and then RC control.

I was wondering, is this any good? how much better can I get without using encoders?
why is it drifting like that when I push it harder? Tunning more P or D doesn't help!
any comments about it would be greatly appreciated

thank again!

Nahua


Hi Nahua, welcome
Thanks for your PM and the nice words
your bot (http://www.youtube.com/watch?v=oM-j-L3Jg7Q ) is balancing pretty well, congratulation  :)

P(i)D tuning seems optimal
The forward/backward drift is typical and normal, it will disappear with your encoder implementation

Are you still using EMG30 motors?? can you quantify backlash ??

Oh... one last comment. Don't be shy, make it taller  ;) ;)
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Nov 18, 2010, 01:39 pm
Hi Kas,

I have an odd motor driver controller on my shield and because of that, my motors have a "resistance" to change state, that is if they are stopped, they need a PWM value over 90 to start and if I tell them to stop it takes time to do so, but they stop spinning for a PWM value less than 30. It works fine for regular robots, but in this case it's hard to compensate. I will hack the driver to change it to regular and see what happens. If it works, I'll redesign the driver.

Regarding the xBee code uploading, sometimes it works, sometime it doesn't. It seems that the original bootloader isn't good for wireless uploading. I should try optiboot and report back.

I am using SF regulated boards for code uploading, one on the robot, connected to the FTDI connector, one connected to the FTDI basic board (5V) through an Tx-Rx inverter (just wires swapped). The DTR line is hardwired to the D3 on the regulated boards, so no hardware modifications needed.  But I think the problem is the 3.3V logic voltage that sometimes is not registered as High by the microcontroller. I found this tutorial usefull to set up the xBees: http://www.ladyada.net/make/xbee/arduino.html

Anyway, I had better success with ATmega168 (uploading at 19200) than with ATmega328 (uploading at 57600). Currently, my xBees are set to 19200 and the signal from the Acc and Gyro looks clean. For the moment, I upload code with the regular FTDI board and after that plug the xBees to see the data on the screen.

BTW Patrick, I can't get your new revision of the wonderful processing sketch... the link is broken...
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 18, 2010, 03:31 pm
@Ro-Bot-X

I found some serious bugs that made the loop time take up to 60 milli/seconds in the Arduino, so I will fix that later this day and upload a modified version there this is fixed.

You will also see the loop time in that version and be able to set how often the Arduino should send information to the pc. (Update rate of the GUI).

I will also try to add the possibility to change the motor offset on line if your motors behave differently..

If you find any bugs or improvements for the KasBot V1.x serial code and the Balancing bog GUI please post it here...
Title: Re: Balancing robot for dummies
Post by: gioscarab on Nov 18, 2010, 07:09 pm
HI guys look @ this I am working with another type of PID, I use a variable time PID.
I save the time of every loop and I use it to give a "weight" to every single acquisition.
It works very well:
http://www.gioblu.com/tutorials/programmazione/102-che-cose-lalgoritmo-pid

I think this solution works better then fixed time pid
Title: Re: Balancing robot for dummies
Post by: kas on Nov 19, 2010, 07:50 am
Hi gbm,

I followed the link, looks quite interesting. PID algorithm and tuning are definitly the key points for efficient balancing.
In our global village, you have to bite the bullet and use the Dominant Language (used to be French some centuries ago  ::)).
gbm, care to prepare a "variable PID for dummies" and post it here ???
Title: Re: Balancing robot for dummies
Post by: kas on Nov 19, 2010, 06:09 pm
Quote
@kas
Are you happy with the xbee kit? How are you implementing it on the kas bot? Do you have the shield on the bot and the USB explorer card in the PC?


Quote
Great progress on the balancing part, but is anyone working on remote control for this Arduino robot? I

built several balancing robots using NXT and VEX kits and this is a great learning project, but I hesitate to spend

$400 until I see that someone has implemented remote control of some kind. I think that just the robot without RC

is not complete.

keep it up guys, you are doing a good job!


I am very happy with Xbees, they work beautifully   8-)

On the bot side, shield with Gyro/Acc combo and Xbee
(http://imgur.com/MzEeV.jpg)
(Some wires have been removed for better viewing)


For debugging and PID tuning, the Xbee Explorer USB dongle is directly connected to the PC
Same as a USB cable, except there is no cable  ;), which is useful for a moving bot
(http://imgur.com/KV0Ez.jpg)


For R/C I use a Nunchuck joystick plugged on the Xbee wireless shield
(http://imgur.com/1g0ZS.jpg)
The micro joystick which is soldered on the shield will be used as trims for robot translation and rotation

Everything is up and running
Title: Re: Balancing robot for dummies
Post by: rwehner186 on Nov 21, 2010, 05:51 am
@Kas

One simple method that I have discovered from:
http://www.rtfa.net/2009/01/02/arduino-ir-remote-control-more-advanced demonstrates how to control the arduino using an IR remote. I bought the IR receiver and universal remote from Radioshack for $20.

The code to get everything working is included at the site above. It basically reads the IR signal from the remote and translates each button's unique pulse combination into an integer. Using a case structure, you can specify commands to execute for each button. Should be pretty straightforward.

-Ross
Title: Re: Balancing robot for dummies
Post by: kas on Nov 21, 2010, 01:08 pm
Hi Ross

Thanks for the link  :)
IR memotes are nice and inexpensive for R/C
They are very good at ON/OFF control, but for Analog/proportional they lack flexibility.
In our case, analog joysticks are much better for the job

I will post new tutorials (encoders, R/C control) later this week
Title: Re: Balancing robot for dummies
Post by: nahua on Nov 21, 2010, 04:55 pm
Quote
Hi Nahua, welcome
Thanks for your PM and the nice words
your bot is balancing pretty well, congratulation  

P(i)D tuning seems optimal
The forward/backward drift is typical and normal, it will disappear with your encoder implementation

Are you still using EMG30 motors?? can you quantify backlash ??

Oh... one last comment. Don't be shy, make it taller  


Hey Kas...

Yes, Im using the EMG30 motors andbacklash is quite low, just a couple of millimeters with the 10 cm wheels. I can make a video if you need so see it with your own eyes.

Will try to add more deckers and make it taller soon :)

Im starting to implement encoders and then make:
torque = P1 + D1 + P2 + D2
(1 = error from sensor --> complementary filter)
(2 = error from encoders)

I'm going crazy now adjusting PIDs so I started graphing things in Processing, still couldn't get anything as I wanted... maybe a PID expansion tutorial needed?

thanks god you are making more tutorials about RC and encoders, cant wait for them!

thanks for the recommendations!
Title: Re: Balancing robot for dummies
Post by: osmaneralp on Nov 21, 2010, 11:48 pm
Many thanks to everyone, especially kas, for this awesome thread. I'm building my own balancing robot, and I've learned a lot here. I think I've read every post, but I might have missed something. Can someone please give me some guidance on these points:
1. Is a 10ms loop sufficient, or should I try to go faster if my peripherals allow it?
2. I can't access the A2D on my CPU. I need to purchase a separate A2D chip to read the gyro. Is a 10 bit A2D sufficient, or should I go 12 bits?
3. Can anyone suggest a good standalone A2D for a balancing robot?

TIA. I hope I can post a movie of my robot balancing soon!

--Osman
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 22, 2010, 06:40 am
@kas

I have now bean test running the robot with the battery you showed me on ebay.. I have just bean playing with the PID parameters for a hour or so but I'm not getting the robot to stand "still".

It was to long a go I worked with PID so I need to read up a little..

The robot Is starting to get heavy, 2.359 kg  :o, don't know if that can give me some troubles..

I will try some more and post a video of my success or failure..
Title: Re: Balancing robot for dummies
Post by: kas on Nov 23, 2010, 08:06 am
@tecabe
Quote
Im starting to implement encoders and then make:
torque = P1 + D1 + P2 + D2
(1 = error from sensor --> complementary filter)
(2 = error from encoders)
You are on the right track  ;)
More info later this week



Hi Osman, welcome, thanks for your comments
Quote
1. Is a 10ms loop sufficient, or should I try to go faster if my peripherals allow it?
100hz is plenty. I adjusted loop time up to 200Hz without any significant change in attitude.
Try for yourself and make sure that your processor can cope with the situation (STD_LOOP_TIME - lastLoopUsefulTime should be >o), especially if you send wired or wireless debug info to your PC

Quote
2. I can't access the A2D on my CPU. I need to purchase a separate A2D chip to read the gyro. Is a 10 bit A2D sufficient, or should I go 12 bits?
Go for a 12 bit ADC

Quote
3. Can anyone suggest a good standalone A2D for a balancing robot?
I strongly suggest MCP3204 or 3208 (http://www.datasheetcatalog.org/datasheet2/c/0hkaplpi05zut805oz5ixzccqpcy.pdf). I am currently testing this chip, source code available on request

(http://i.imgur.com/dpLw5.jpg)



@Patrik
Quote
The robot Is starting to get heavy, 2.359 kg  , don't know if that can give me some troubles..
You built a balancing sumo  :) :)
Keep us aware
Title: Re: Balancing robot for dummies
Post by: osmaneralp on Nov 23, 2010, 04:57 pm
@kas
Quote
I strongly suggest MCP3204 or 3208. I am currently testing this chip, source code available on request

That's good to hear. I have a 10 bit MCP3008 that I wan going to use on another project. I start with that and swap it out when I can get the 3208.

How did you read the SPI bus? Does your processor support SPI, or did you implement a bit banged solution?

Many thanks!
--Osman
Title: Re: Balancing robot for dummies
Post by: jondaeh on Nov 23, 2010, 09:07 pm
Thanks for answer kas, I'm sorry for really slow answer. I actually didn't notice that the thread continued on the next page:P

I don't have much time for debugging these days, as I have exams. But soon is the christmas vacation, and that means I have plenty of time experimenting 8-)

kas:
Quote
Finally make sure your gravity compensation for the "vertical" accelerometer is OK


I have "tuned" the accelrometer Z-axis so that the angle calculated from accelrometer is zero when the robot is at equalibrium. Then I tilted the robot physically 45 degrees(measured with tool) and checked that the angle was about correct. The angle read from the accelrometer was approx 2 degrees at 45 degrees, which shouldn't be a problem as this error will be very little at the small angles from the robot will(hopefully) work around.

The gyro sign is also correct, positive accelrometerangle -> positive gyro output. The only problem is that when integrating the gyro without adjusting with accelrometer(no kalman or complimentaryfilter), the integrated angle is way to small. However, dividing the sensitivity constant for the gyro(which makes the reading 3 times bigger), the angle seems about right. But I don't like solutions like this when I don't know why the reading is wrong in the first place..

Anyways, here is a video of the balance robot attempting to balance. I suspect that the estimation of the angle is a bit slow, so that the robot is a bit "late" in compensating.

Video: http://www.youtube.com/watch?v=w6XP9V11lhM

The video also shows the Java-based software on the computer which is used to tune the controller on the roboot wirelessly over XBee. By the way, it is running state-feedback instead of tradisional PID control, that's why the text on the software is K1,K2,K3 instead og P,I and D.  

Will be back with more info about the angleproblem when I'm done with my exams, until then thanks for the help! I appreciate it :)

Jon, Norway
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 24, 2010, 07:14 pm
@kas

A balancing sumo is the right word for it and it has some trouble to be able to stand. I will try to lighten it when I get some more time for it.

I started to check the differences of the motors by using the encoders to try to see the distance the motors turned in a constant time and get a factor for the diff. I drive the motors in both directions in four different speeds 50, 100, 200, 250..

I made a sketch for the Arduino and a GUI sketch in processing based on the Balancing Bot GUI, and thought that this cant be so hard to do just see the diff. and calculate a factor for the faster motor but that was not the case..

The first run gave me the results like in the image below, where the motors behaved very different in forward to backward motion. When going forward for a time constant(2000ms) and then sending the drive_motor(0) the motor continued running in free spin for a short while. But when running backward  the motor brakes for a short while.

//image See the blog post on my blog (http://www.x-firm.com/?p=250)

Speed   L        R
255       7662       : 7413
-255       -5516       : -5541

150       6582       : 6386
-150       -4901       : -4872

50       3342       : 3291
-50       -2747       : -2705

As you can see the behavior is very different in the two directions..

I changed the drive_motor function like below and tried again and the result was better but is I going about this in the right way?
Code: [Select]

int Drive_Motor(int torque)  {
 if (torque > 0)  {
   // drive motors forward
   digitalWrite(InA_R, LOW);
   digitalWrite(InB_R, HIGH);
   digitalWrite(InA_L, LOW);                      
   digitalWrite(InB_L, HIGH);        
   forward = true;    
 }else if(torque < 0) {     // drive motors backward
   digitalWrite(InB_R, LOW);
   digitalWrite(InA_R, HIGH);                      
   digitalWrite(InB_L, LOW);
   digitalWrite(InA_L, HIGH);                      
   torque = abs(torque);
   forward = false;
 }else{
   if(forward){
     digitalWrite(InA_R, HIGH);                      
     digitalWrite(InB_R, LOW);
     digitalWrite(InA_L, HIGH);                      
     digitalWrite(InB_L, LOW);
   }else{
     digitalWrite(InA_R, LOW);                      
     digitalWrite(InB_R, HIGH);
     digitalWrite(InA_L, LOW);                      
     digitalWrite(InB_L, HIGH);
   }
 }
 //if(torque>5) map(torque,0,255,30,255);
   analogWrite(PWM_R,torque * motorOffsetR);
   analogWrite(PWM_L,torque * motorOffsetL);
   Serial.println(torque,DEC);
}


Now it looks like the Arduino and the motor controller is behaving in the same way in both directions. I need to take some more readings to see if the differences that are left are linear in some way.. It feels like there are a difference between the motors in both directions but also that both motors are going faster forward, and that I think is created by the motor control??..
See image below..

//image See the blog post on my blog (http://www.x-firm.com/?p=250)
Title: Re: Balancing robot for dummies
Post by: X-firm on Nov 24, 2010, 11:01 pm
I'm not entirely happy with the backlash in my motors and I wounder what you guys thinks of using two brushless motors and some belt transmission?? Like in beautifulsmalls robot..
Title: Re: Balancing robot for dummies
Post by: kas on Nov 25, 2010, 08:06 am
@Osman
Quote
How did you read the SPI bus? Does your processor support SPI, or did you implement a bit banged solution?
ATmega168 and 328 are supposed to have built-in hardware support for SPI
I still manage the MCP3208 by the bits
Look here (http://www.arduino.cc/playground/Code/MCP3208) and here (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1206784528/0)

What processor are you using ??



@Jon
Shall I understand that you build robot at school??  lucky guy :D
You are also lucky because robot technology will develop quickly, and you will probably never fear unemployment...
I understand you are developing a different software, but still, can you check your raw sensor value and make sure that you get a symetrical pattern, as per photos in reply #113
Looking at the bot height, I suspect you are not a shy guy  ;)
Anycase, good luck for your exams. I hope you will come back later and give us additional infos on state-feedback.


@Patrik
Belt transmission is probably the way to go for zero backlash, but not easy to implement
Are you using the Pololu 350RPM motors ??
I confirm that backlash is noticiable, but it doesn't prevent proper balancing.

Quote
I started to check the differences of the motors by using the encoders to try to see the distance the motors turned in a constant time and get a factor for the diff. I drive the motors in both directions in four different speeds 50, 100, 200, 250..
I will read your blog entry and come back soon
Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Nov 26, 2010, 08:52 pm
cheap velocity encoders for belt transmission:
just connected adns-7550 mouse encoders , SPI-4wire, to my DsPic board, sub $10 for chip and lens, (laser diode integrated into 7550) Mouser. soldering a bit fiddly, with the very small fet on the data sheet, smaller than sc70!!.
but, excellent results with the belt sprayed with metallic spray. the 7550 reports surface quality to aid positioning, 60 inch/sec max rate , within balance robot thresholds, 2000 counts per inch, better than $30 heds encoders. Ive also tried it on the wheel wall (gold sprayed) and get excellent velocity data. 2.5mm airgap lens to moving surface but not critical.
format of spi_read(address,data)  (hex AA,DD)
ADDRESS : 7550 spi read is MSB=0, write MSB=1, this is different from BMA180,ADXL345? which has read as MSB=1WR MSB=0.


// ++++++++++++++++ mouse 7550 ++++++++++++++++++++++++++++++++
int mouse7550_init() // uses slave select A9
{

     spi_write(0x3a5a);  // power on reset
pause(10);
     spi_read(0x2e00);   //observation reg read clears it
pause(10);
     spi_read(0x0200);   //read form reg 2
      spi_read(0x0300);   //read form reg 2
      spi_read(0x0400);   //read form reg 2
      spi_read(0x0500);   //read form reg 2
     spi_write(0x3C27);   //reserved
     spi_write(0x220A);   //reserved
     spi_write(0x2101);   //reserved
     spi_write(0x3C32);   //reserved
     spi_write(0x2320);   //reserved
     spi_write(0x3C05);   //reserved
     spi_write(0x37B9);   //reserved

     spi_write(0x1a40);   //laser cfg0 , range NNxxxxxx
     spi_write(0x1cff);   //laser power 8 bits ff=100%
     spi_write(0x1d00);   //complement of reg 1C,laser powert
     spi_write(0x1fB0);   //complement of D7,D6,cfg0

     spi_write(0x1260);   //resolution 0x xNNx xxxx
     // resolution  NN =0 =400
     //NN=01 = 800 , NN=10 = 1200 , NN=11=1600
     
return 0;
}

//+++++++++++++++++++++++++ mouse encoder ++++++++++++++++++++++
int mouse_x_read()
{
int temp;
unsigned int low,high;
int x;

low=spi_read(0x0300);   //read form reg 2
temp=spi_read(0x0400);  
high=spi_read(0x0500);   //read form reg 5

low=(low & 0x00ff);
high=((high & 0x0f0) <<4);

x=(high | low);

return (x);
}
Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Nov 26, 2010, 09:38 pm
comment on brushless motors vs coreless,
ive been looking at brushless and im still unsure, I think the fundemental question for balancing robots is "what is smallest torque" and also "the highest (stall) torque". for that perfect balance point a motor that steps is no good, you need a motor that can be adjusted by 1degree/sec and also can do 200mNm and reverse in 1/100 s , the problem with the "off the shelf " brushless motors is the driver controls that have set levels of acceleration, as Homer says "I want it now" which only coreless can give you. The sensor type of brushless are getting better but ive emailed the best and they cant do what coreless can, they can do low rpm but only with big amps! they are still a stepper at heart.  maybee im being a pedantic git and to balance you don't need 0.1 deg/sec control but thats just me.  just bought 2 RE-max graphite,22W, 16Amax, VNA30A dual control fron spark and ITG-3200 for my next 4-wheel balancer, I need to learn about li-po
whats that coming over the hill, is it a monster, its a BF952.
Title: Re: Balancing robot for dummies
Post by: kas on Nov 27, 2010, 09:08 am
- plain DC motors
- coreless motors
- brushless motors
- continuous rotation servos
- spurs gearbox
- planetary gearbox
- belt transmission
- ...
the perfect cinematic chain still needs to be identified  :-?

patrik, I also noticed that motors have different forward/backward response.
Make sure that you use the total potential power of your motor. Monitor you PWM output, and check that value is not clipped by the K parameter.

The answer is software or hardware related (or a combination of those)
Low RPM with high torque can be obtained through an additional PID loop
see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0

Backlash may be addressed within the code
Look at this one:
(http://i.imgur.com/wMyJe.jpg)
http://www.youtube.com/watch?v=zyslsBd8J5c

cheap motors with plastic gears, small wheels, excellent results  :o
I hope Dominic will jump in and share his experience


Quote
just bought 2 RE-max graphite,22W, 16Amax, VNA30A dual control fron spark and ITG-3200 for my next 4-wheel balancer, I need to learn about li-po
whats that coming over the hill, is it a monster, its a BF952.
beautifulsmall, This new digital output gyro looks promising.
Please let us have a link to the motors you mentioned

Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Nov 28, 2010, 02:58 am
Kas, i guess i have given the impression that only expensive motors are good for balancing , im sure this is not true, to get balance any motor geared , plastic or whatever , will get there.
I dont want to balance, I want not to be knocked over!
60:1 plastic gears will never cut the mustard., i know im over egging the pudding with coreless, but you cant get that reaction from any other motor, they are astoundingly fast to change , they will buzz at 1KHz and the bot dances like a wild thing. I think 15:1 gearbox with a standard DC motor will give good balance results. I dont want that, I want to beat a cat, to hit a ball , 60g tennis ball at 4m/s. we can do this, you me and everyone here. Blues brothers. :D
Title: Re: Balancing robot for dummies
Post by: kas on Nov 28, 2010, 08:13 am
Quote
I dont want that, I want to beat a cat, to hit a ball , 60g tennis ball at 4m/s. we can do this, you me and everyone here. Blues brothers.
+1  My final goal is definitly perfect balancing - long is the road   ::) ::) ::)
Title: Re: Balancing robot for dummies
Post by: kas on Nov 28, 2010, 08:20 am
      [size=14] ** Balancing robot for dummies **[/size]


[size=14]Part six: Motor encoders and V2.0 code[/size]

Before even thinking about R/C wandering, a  well-mannered balancing robot has to learn to stand still  >:(
Without encoders the best achivable attitude is  Nahua implementation (http://www.youtube.com/watch?v=oM-j-L3Jg7Q)
The bot is aware of his vertical tilt angle, he does know anything about his position.
Lets add motor encoder(s)

Look here (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0) and here (http://www.pololu.com/catalog/product/1443) for encoders basics:

(http://imgur.com/BYC2M.jpg)
A two-channel Hall effect encoder is used to sense the rotation of a magnetic disk on a rear of the motor shaft.
The quadrature encoder provides a resolution of 64 counts per revolution of the motor shaft.
To compute the counts per revolution of the gearbox output, multiply the gear ratio by 64.
The A and B outputs are square waves from 0 V 5 Vcc, 90° out of phase.
The frequency of the transitions tells you the speed of the motor, and the order of the transitions tells you the direction.
The oscilloscope capture shows the A and B (yellow and white) encoder outputs.

(http://imgur.com/YyEXv.jpg)

By counting both the rising and falling edges of both the A and B outputs, it is possible to get 64 counts per revolution of the motor shaft.
Using just a single edge of one channel results in 16 counts per revolution of the motor shaft (16 X 29 = 464 counts per wheel rotation).


For the code implementation, first create an interrupt based on DI3 state change (see interrupt Arduino reference (http://arduino.cc/en/Reference/AttachInterrupt ))

Code: [Select]
void setup() {

 ..................

 pinMode(encodPinA1, INPUT);
 pinMode(encodPinB1, INPUT);
 digitalWrite(encodPinA1, HIGH);                    // turn on pullup resistor
 digitalWrite(encodPinB1, HIGH);
 attachInterrupt(1, rencoder, FALLING);
}


the rencoder function is called 464 times per wheel rotation, keep it small !!!
Code: [Select]
void rencoder()  {              // pulse and direction, direct port reading to save cycles
 if (PIND & 0b00000100)    count++;                  
 else                      count--;        
}

if (PIND & 0b00010000)  is equivalent to  if(digitalRead(2)==HIGH)
Accessing DI's state through the PIND register is just faster

The count variable is injected in the PID function in the same way as we did for the bot angle:
- as such (wheel rotation)
- as its first derivative (wheel speed)

Code: [Select]
int updatePid(int targetPosition, int currentPosition)   {
 int error = targetPosition - currentPosition;
 pTerm = Kp * error;
 integrated_error += error;                                      
 iTerm = (Ki/5) * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
 dTerm = Kd * (error - last_error);                            
 last_error = error;
 pTerm_Wheel = Kp_Wheel * count;          
 dTerm_Wheel = Kd_Wheel * (count - last_count);                            
 last_count = count;
 return -constrain(K*(pTerm + iTerm + dTerm + pTerm_Wheel + dTerm_Wheel), -255, 255);
}

After properly tuning the 6 ( :o) parameters of the PID algorithm, your bot should finally stand still.

I will send kasBot V2.0 to Patrik for publishing in his blog (http://www.x-firm.com/?page_id=145)
Patrik please add the encoders PID parameters as tunable values in BalancingBotGUI v1.3
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Nov 28, 2010, 03:13 pm
So Kas, you are using only one encoder, right? And you are using it to count only one transition of one channel, right? I suppose it's because you have many counts generated per wheel rotation. In my case, I have only 141 counts in single channel, single transition. I use 2 transitions (change) to get double of that, but I suppose I shoul use the full quadrature to get four times 141 to get close to what you're getting with your setup.

Anyway, to understand better, any time an interrupt is generated by pin D3 (interrupt 1) you check the logic state of pin D2 (interrupt 0) and increment or decrement the counter to get the speed and direction at the same time. I do the same thing, but I use a regular I/O pin for the second channel of the encoder and the interrupt 0 for the other motor's encoder. Then I compare the 2 encoder counts to see if the motors are running at the same speed or need to be compensated somehow, usually I speed up the slower motor. But this is for a regular robot. Is it worth the trouble to go all this way for the balancing robot?
Title: Re: Balancing robot for dummies
Post by: kas on Dec 01, 2010, 02:58 pm
Hi Ro-Bot-X,

I use one encoder, one channel only. As you say, 464 counts per wheel revolution is more than adequate.
In your case, quadrature (look here (http://www.mindspring.com/~tom2000/Delphi/Codewheel.html)) will definitly increase resolution.
I currently use only one interrupt (#1) and check, whithin rencoder, the other channel logic state (regular I/O).
I make a first right/left motor speed adjustment within code, and fine tuning through micro joystick trim:
(http://i.imgur.com/mpXm1.jpg) (http://imgur.com/mpXm1)

The second encoder can also be used for this task, but I hesitate to create a second interrupt within the code.
Title: Re: Balancing robot for dummies
Post by: kas on Dec 05, 2010, 05:56 pm
If your boot tells you he can fly  :P, [size=14]don't trust him[/size]
The takeoff from the mezzanine was OK, only the landing 3 meters below did create problems  ;)

(http://i.imgur.com/f3eYz.jpg)

Well c'est la vie  :-/


I have to stop active development for some days (Xbee wireless RC is 90% completed)
For those who have started building a balancing bot, please register and share your design and/or your source code.
Any new development ideas are also welcome.

I will be back soon...
Title: Re: Balancing robot for dummies
Post by: X-firm on Dec 06, 2010, 07:44 pm
Oh no! That didn't look so nice.. Hopefully it was not to much damage to more then the obviously.  :-[
Title: Re: Balancing robot for dummies
Post by: GuzZzt on Dec 07, 2010, 10:49 am
Here is my first try to tune the PID by following your tuning tips (thank you very much for that). I have so far not used the values from the encoders because I want a little moer stability before I do that. Do you have more tips on how to find the best values? I have also tried to google about it but it is hard to follow when it is about other stuff than balancing bots...

http://www.youtube.com/watch?v=wt-ICoBT5L0
Title: Re: Balancing robot for dummies
Post by: X-firm on Dec 07, 2010, 03:25 pm
@GuzZzt

Looks like a good start, could you take some pictures on the bot and post your construction.. Do you have some instructions and pictures on the bot on some blog or on LMR?

Title: Re: Balancing robot for dummies
Post by: kas on Dec 07, 2010, 06:57 pm
Quote
Hopefully it was not to much damage to more then the obviously
Thanks for the empathy, I just needs a new frame, a motor (bent shaft) and a wheel

(http://i.imgur.com/iN6jg.jpg)
Back to (nearly ;)) square one


Quote
I have so far not used the values from the encoders because I want a little moer stability before I do that.
Do you have more tips on how to find the best values?

Time for encoders  ::)
As mentioned by Patrik, please post photos of your setup.
I am curious about your motor positioning, looks like a belt transmission ??
Title: Re: Balancing robot for dummies
Post by: GuzZzt on Dec 07, 2010, 09:43 pm
Sure, here are some photos. I plan to write a little more on my site after I'm finished with the main stuff.

I use the same wheels and motors (but only 150RPM) as you do from pololu.com. the belt driving was inspired by the n-bot robot. If you look closely can you see that I in fact don't use an Arduino but a Maple board (an ARM Cortex M3 but the C code is still the same).

(http://www.guzzzt.com/balancing_bot/images/_DSC0086-1.jpg)
(http://www.guzzzt.com/balancing_bot/images/_DSC0088-1.jpg)
(http://www.guzzzt.com/balancing_bot/images/_DSC0092-1.jpg
)
(http://www.guzzzt.com/balancing_bot/images/_DSC0093-1.jpg
)
Title: Re: Balancing robot for dummies
Post by: kas on Dec 08, 2010, 06:26 pm
@ GuzZzt
Quote
I use the same wheels and motors (but only 150RPM)

Your motors may be a bit slow, you may
- try the 90mm wheels if you currently mounted the 80mm ones
- increase the global PID parameter (K)
- use a smaller pulley on the wheels shafts

Please let us have a link for the belts and pulleys hardware
Title: Re: Balancing robot for dummies
Post by: GuzZzt on Dec 08, 2010, 09:52 pm
The mechanics comes from http://www.technobotsonline.com
Title: Re: Balancing robot for dummies
Post by: kas on Dec 09, 2010, 07:34 am
Quote
The mechanics comes from http://www.technobotsonline.com
GuzZzt, are you using T2.5x6mm Timing Belts??
please be more specifific and let me have a direct link to the hardware you actually use (pulleys, belts, shafts, bearing housings)
Title: Re: Balancing robot for dummies
Post by: GuzZzt on Dec 10, 2010, 09:25 am
Here is my shopping list from technobots:
Silver Steel 6mm x 333mm  4426-006 (http://www.technobotsonline.com/silver-steel-6mm-x-333mm.html)
6mm Bearing Blocks pk/2  4604-021 (http://www.technobotsonline.com/6mm-bearing-blocks-pk-2-33654.html)
Zinc Collets 6mm pk/4  4609-060 (http://www.technobotsonline.com/zinc-collets-6mm-pk-4.html)
Pololu Mounting Hub 6mm  4609-106 (http://www.technobotsonline.com/pololu-mounting-hub-6mm.html)
Pololu Wheel 90x10mm in Black - Pair  4611-046 (http://www.technobotsonline.com/pololu-wheel-90x10mm-in-black-pair.html)
Synchroflex Timing Pulley T2.5x6mm 32T  4620-132 (http://www.technobotsonline.com/synchroflex-timing-pulley-t2.5x6mm-32t-33862.html)
Synchroflex Timing Belt T2.5x6mmx180mm  4660-180 (http://www.technobotsonline.com/synchroflex-timing-belt-t2.5x6mmx180mm-34089.html)
Title: Re: Balancing robot for dummies
Post by: kas on Dec 11, 2010, 08:06 am
While my bot is sitting on the shelf, waiting for a new leg, I re read the all thread.  :o :o

Let me thank all the contributors:
  zalo, novice, haha97, thinkaneer, cr0sh, josev, Onions,
  Gibby623, Ro-Bot-X, granyte, killersquirel11, WanaGo,
  Patrik, wowmir, gbm, hoyeungl, T.J.L., pramlie, Jon D.,
  uncle_tom, RWehner, GuzZzt

Some have been rather silent recently, please stop by and share your new positive/negative experience.

For the silent others who helped generating the 28,000 read pages in the 2 threads, please introduce your project (with photos  :)).
Let's establish this thread as a reference for others to find both inspiration and technical information.
Title: Re: Balancing robot for dummies
Post by: psychoul on Dec 11, 2010, 11:45 am
Hi there kas, last week I was searching around for kalman filtering for balancing robot and I came across your thread. I found it very useful and I am gearing up for my own balancing robot.

I am not interested the details so much (i.e. the code) but I found the high-level description REALLY useful. So now that with your help I identified the building blocks of the procedure (data acquisition, smoothing, filtering, control) I am going to start developing.

Personally I lack of the mechanical potential. Not much hardware stores in my country, so everything has to be ordered through the internet which delays the whole procedure a lot!

I should let you know that I am mainly a PIC user, so I will develop for that platform :)

Anyway. Thanks for the info. I will try to be active in the discussion here.
Title: Re: Balancing robot for dummies
Post by: kas on Dec 12, 2010, 08:57 am
Hi psychoul

thanks for joining us

You are right, the processor itself is not the main part of the project.
My initial intention was to describe the process using pseudo code only.
I wanted to avoid people to just "copy/paste" code, and make sure they understand what they are doing  ;).

You are welcome back with questions , and to share your development.

Quote
Not much hardware stores in my country, so everything has to be ordered through the internet which delays the whole procedure a lot!
Short of getting local convenient sources, most of us, in Europe, purchase in the USA (Sparkfun, Adafruit, Pololu...)
Out of curiosity, where are you from ??
Title: Re: Balancing robot for dummies
Post by: psychoul on Dec 12, 2010, 11:34 am
hey kas,

thanks for welcoming me :D

I am from Cyprus (where the temperature dropped under 20 degrees just yesterday...oof)

So you all guys practice the skill of patience when ordering stuff huh!? Its a very bad feeling. I am waiting for 20 days now, for some fast diodes to arrive to build the h-bridge of my robot.

I have already build the brain platform on a breadboard, and some general structure for the algorithm which allows wireless debugging and settings changes through Xbee.

For the chassis, I really liked your idea and I was able to find the materials from a local store :D. The only problem is my motors. I have these http://www.pololu.com/catalog/product/1123 cheap motors. I believe they will be able to balance the bot for tiny angle errors but not if I push the robot. What do you think?

Title: Re: Balancing robot for dummies
Post by: novice on Dec 12, 2010, 12:42 pm
Hi psychoul,

with Arduino compatible boards being so affordable ($24) it would be much easier for you to replicate 'exactly' what kas did.
Title: Re: Balancing robot for dummies
Post by: psychoul on Dec 12, 2010, 12:46 pm
hey novice, well I am not looking into replicating the code. After all the purpose is not to just have a balancing robot but the procedure. The "problem" that I am facing is on the mechanical part of the project, being the delays from the hardware stores. Thanks.
Title: Re: Balancing robot for dummies
Post by: kas on Dec 12, 2010, 06:15 pm
Quote
I am from Cyprus (where the temperature dropped under 20 degrees just yesterday...oof)
I'am jealous  >:(


Quote
The only problem is my motors. I have these http://www.pololu.com/catalog/product/1123 cheap motors. I believe they will be able to balance the bot for tiny angle errors but not if I push the robot. What do you think?
:-? :-? :-?

120:1 Mini Plastic Gearmotor (http://www.pololu.com/catalog/product/1123):  120RPM, 80mA @4.5V (0.36W) 20 oz.in
29:1 Metal Gearmotor (http://www.pololu.com/catalog/product/1443):  350RPM, 300mA @12V (3.6W) 110 oz.in

I am affraid it's a bit short (been there, done that with Solarbotics GM9's)
120RPM is borderline
Motors may not have enough torque for rapid Forward/backward transitions

Make your boot as light as possible (<500g)
use big wheels (>80mm)
and... let us know ;)
Title: Re: Balancing robot for dummies
Post by: psychoul on Dec 12, 2010, 06:51 pm
Don't be jealous! Quite the opposite! We are (or I am at least!) quite bored of all the sunlight and the heat since June. Its Xmas! it should be snowing and be cold :D

Yes you are right about the motors but I am bit short on money currently so I would like to test the motors before upgrading.

Personally I am very interested in control (control theory) implementations. I found the balancing robot to be an ideal objective to play around with different control ideas, PID, fuzzy logic, adaptive etc etc. But I need to establish a good platform first before digging into those areas.

How about you. It seems you are interested about control as well. Any other interesting projects or something?
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Dec 12, 2010, 09:33 pm
I have tried to use these motors: http://www.solarbotics.com/products/gm12a/ which have 169RPM free spinning and believe me they are not fast enough to balance the robot. I will get faster motors and try again. The idea was that I have built a Compact Arduino robot and wanted to see if it can balance. Perhaps using faster Pololu micro motors (40:1 has like 440 RPM) could make it work. I was extremely busy with work lately and did not make any improvements on my bot. I'll make a test bed for some a little faster motors (still not fast enough) that I already have and see how that's going to work. All my money went for other projects at the moment (multi servo robot) so this will have to wait.
Title: Re: Balancing robot for dummies
Post by: psychoul on Dec 12, 2010, 10:14 pm
I have seen balancing robots using simple servo motors which are pretty slow compared to other DC motors. Is the motor choice so important afterall?
Title: Re: Balancing robot for dummies
Post by: Onions on Dec 13, 2010, 05:54 pm
I am planning on building a similar robot soon, so I was wondering
where you got the wheels? (A UK supplier would be nice!)

(sorry for posting this on the wrong thread!)
Title: Re: Balancing robot for dummies
Post by: kas on Dec 13, 2010, 06:31 pm
Quote
How about you. It seems you are interested about control as well. Any other interesting projects or something?

I worked for a process control company for many years. We used to produce and sell DCS (Distributed Control Systems) and Plant Automation sensors with 4-20mA interface.
I started a new exciting :o :o :o project also based on control/balance.
I wont talk about it now to avoid polluting this thread  ;)


Quote
I have seen balancing robots using simple servo motors which are pretty slow compared to other DC motors. Is the motor choice so important afterall?
My first balancing robot was based upon servo motors, using a Parallax Basic Stamp processor.

(http://i.imgur.com/nYS9m.jpg)
http://forums.parallax.com/showthread.php?103850-Yet-another-balancing-robot&daysprune=-1

Could balance for minutes only; couldn't recover from a slight push.
I tried to adapt an IMU 5DOF sensor on this same design, but never succeeded to make it balance. :(

Look at beautifulsmall (http://www.youtube.com/watch?v=Kd2kJxBkPmk) or RoObi (http://www.youtube.com/watch?v=zyslsBd8J5c) design to understand what stability reallly means  :P
(also this (http://play.killerfrog.com/l0k1e8k1a6/Botka-The-Barely-Standing-Robot.html)  and this (http://www.youtube.com/watch?v=RcNoL3vtXFs))
Title: Re: Balancing robot for dummies
Post by: jondaeh on Dec 13, 2010, 06:57 pm
(Last post #115)

Allright, I'm done with my exams, and ready to make that robot balance :D

@kas
Yes, we're(me and my group members)is building the robot at school. It's a part of a "real time computer science" class. Even though it became almost more of a cybernetics project rather than a programming one. As the class is now finished, I decides I wanted to continue with the robot at home, as I would love to see it balance:)

The reason we built it so high is that it is easier to make a matheatical model of a pendulum with a high center of gravity. In theory it is also easier to balance a high center of gravity rather than a low one, as it will get a bigger momentum of inertia around the pivot point(and then fall slower). The drawback comes when the robot is out of equalibrium with a small angle. The wheels will have to travel a longer distance faster to compansate for the error, compared to if the center of gravity had been low(Think pythagoras and you will see it). This implies that the angle measurement must be accurate and fast, and this is where we believe our(now mine :P ) problem lies.

I have by the way abandonened the Fez Domino, as we had lot of problems with Visual Studio freezing when downloading the program to the Fez, and a lack of some liberary classes for byte-level operations. Pluss, my computer wouldn't download programs to the Fez at all..:P So I swappped it for an Arduino, and I'm back on track :)

However, my gyro problem continues on, as I'm trying to integrate the angle to verify that the gyro and real(accelrometer) angle got the same scale. Still, the integrated angle is about 33 - 50% of what it should be. I have connected the supply for the IMU to the 3.3V "pin" on the arduino, and also jumped it to the aref. I use the NOT op-amped output from the gyro. I use the factor 1.6(as suggested by kas in post #8) to get the read value in deg/sec.  

I attach the program I use to integrate the gyro at the end of this post, so if anyone is interested it would be great to get some feedback! I use analogpin 5 for the gyro, and the autozero is attached to analogpin 2. Just change this to match your setup. The accelromteres is not used. If you use the op-amped gyrooutput, "gyroSensitivity" should be chenged to 9.1.

I ain't got anywhere to host the files for download, so this post becomes a bit long :/

If you want to try the program, you will have to paste the different modules into different files.

Great work on your robots out there folks! :)

Jon, Norway



The "loop". This is where the integration is done:

Code: [Select]
#include "IMU.h"

IMU imu;
float angle = 0;


void setup()
{
 Serial.begin(9600);
 imu.autoZero();
 imu.getGyroOffset();
}



void loop()
{
 
 float gyro = imu.getGyro();
 angle += gyro * (112.0F/1000.0F); //The sampletime was measured with millis(), and then hardcoded.
 
 Serial.println(angle);
 delay(100);
}


The headerfile for the IMU class:

Code: [Select]
class IMU
{
 public:
 IMU();
 void autoZero();
 void getGyroOffset();
 float getGyro();
 int getAccX();
 int getAccZ();
 
 private:
 int gyroPIN;
 int accxPIN;
 int acczPIN;
 int azPIN;
 int gyroOffset;
 int accXOffset;
 int accZOffset;
 float gyroSensitivity;
 float toDegrees;
};


And the implementation for the IMU class. This is where the gyro is read, and then scaled to deg/sec:

Code: [Select]
#include "IMU.h"
#include <WProgram.h>

 
IMU::IMU()
 {
   gyroPIN = 5;
   accxPIN = 2;
   acczPIN = 4;
   azPIN = 2;
   gyroSensitivity = 2.0F;
   gyroOffset;
   accXOffset = 0;
   accZOffset = 0;
   toDegrees = 3.22F / gyroSensitivity;
   analogReference(EXTERNAL);
 }
 
 void IMU::autoZero()
 {
   digitalWrite(azPIN,LOW);
   delay(1);
   digitalWrite(azPIN,HIGH);
   delay(1);
   digitalWrite(azPIN,LOW);
   delay(1);
 }
 
 void IMU::getGyroOffset()
 {
    long biases = 0;
    for (int i = 0; i < 1000; i++)
     {
         biases += analogRead(gyroPIN);
         delay(5);
     }
     gyroOffset = (int)(biases / 1000);
   }
   
   
   float IMU::getGyro()
   {
       float gyro = (float)(analogRead(gyroPIN) - gyroOffset);
       //if ((gyro <= 2) && (gyro >= -2)) gyro = 0;
       return gyro * toDegrees;
   }

       
   int IMU::getAccX()
   {
       return (analogRead(accxPIN) - accXOffset);
   }

   int IMU::getAccZ()
   {
       return (analogRead(acczPIN) - accZOffset);
   }





Title: Re: Balancing robot for dummies
Post by: jondaeh on Dec 14, 2010, 01:35 am
Okay, I actually solved my problem. I had forgotten to set the AutoZero pin to OUTPUT. This probably made it float, and messed up the measurement. Now the integrated angle is right! :)

However, this won't solve the integration problem on the FEZ Domino and the PIC32, as the AZ-pin were set as output there. But as the error in the integrated angle is about the same as I had with the floating pin it may give an idea of where to start looking! I won't discuss more about these microcontrollers in this thread, but if anyone struggles with this problem I will gladly try to help :)



Jon.
Title: Re: Balancing robot for dummies
Post by: kas on Dec 16, 2010, 07:33 am
@ Jon D
glad to see that you solved your problem
I like your object orientated way of programming, using classes
My bot is currently belly up, I will try your code as soon as I can.
Not sure if the words "FEZ" and "Domino" are allowed on this site  ;)


@ Onions
Hi welcome on the right thread  ;)

Quote
where you got the wheels? (A UK supplier would be nice!)
I got my wheels here (http://www.pololu.com/catalog/product/1437)
I am not familiar with UK suppliers (only this one (http://www.technobotsonline.com/wheels-and-propellers.html) , submitted by GuzZzt)
Others may jump in with additional links
Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Dec 17, 2010, 03:03 am
Hello, the algorythams used so far give a balance for a fixed vertical but im interested in a way of constantly updating this. ie, not using vertical as the reference but using velocity =0 as a goal, im no mathematition but i can see angle is not fixed as any side load will offset this, and any robot that will be usefull needs to allow for new  uncentered loads..maybee a completely new algorythm is needed like fuzzy??, which i understand is basically a weighted decision tree, which seems to me more like a look up table. but maybee that is what is needed, a learning table that can be overwritten. I know ive spent many hours adjusting parameters which is impractical in a comertial product and the slight variations in a comertial product would dont allow fixed parameters, like wo-wee balancing bot was pulled from the market?? im just guessing because it didnt have the fire power to calibrate in a realistic time. Its nice to look at murata and honda and still see there no better than us amateurs!.im very impressd by the recent micromouse maze solvers in 11 secs. but back to balancing, even my dspic 33 at 1.7dmips runs out of steam when balancing and doing math for mapping / need the blackfin 592 as co pro, then get some visual data , and we are really talking about a bot that can manouver and follow a ball.but blackfin is not accesable !!! to the masses , ive got one and i cant bloody program it .
its a nice data in , data out chip but the sw is $7000 ! stupid company,
Give me the tools and ill build you a pyramid ( dohh, its dark in here) ;D
Title: Re: Balancing robot for dummies
Post by: psychoul on Dec 17, 2010, 07:35 am
By my understanding, in order to control the velocity (and make it zero according to you) you need a way to measure it or approximate it. The accelerometer/gyro setup gives you angular acceleration and angular velocity which is created because of the rotation of the chassis of the robot around the wheels axis. If I understand it correctly, you cannot get any velocity information out of that.

One sensor that is used for approximating the velocity of a robot is the wheel encoders (mentioned a while back in this thread), where you get revolutions/time which is velocity.

You mentioned fuzzy, I believe that is just a different way of controlling the input. If you don't have the correct feedback you can't control it anyway.
Title: Re: Balancing robot for dummies
Post by: kas on Dec 19, 2010, 04:33 pm
Quote
Hello, the algorythams used so far give a balance for a fixed vertical but im interested in a way of constantly updating this. ie, not using vertical as the reference but using velocity =0 as a goal, im no mathematition but i can see angle is not fixed as any side load will offset this, and any robot that will be usefull needs to allow for new  uncentered loads...
velocity = 0, or for RC control, velocity = "RC's value"
I like your idea of controlling velocity rather than angle.

Thinking twice, in fact, with encoders, PID algorithm is fed with both angle and velocity data:  :-?
Code: [Select]
int updatePid(int targetPosition, int currentPosition)   {
 int error = targetPosition - currentPosition;
 pTerm = Kp * error;
 integrated_error += error;                                      
 iTerm = (Ki/5) * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
 dTerm = Kd * (error - last_error);                            
 last_error = error;
 pTerm_Wheel = Kp_Wheel * count;          
 dTerm_Wheel = Kd_Wheel * (count - last_count);                            
 last_count = count;
 return -constrain(K*(pTerm + iTerm + dTerm + pTerm_Wheel + dTerm_Wheel), -255, 255);
}


I am not too familiar with fuzzy logic; do you have code snippets to submit??



Quote
By my understanding, in order to control the velocity (and make it zero according to you) you need a way to measure it or approximate it. The accelerometer/gyro setup gives you angular acceleration and angular velocity which is created because of the rotation of the chassis of the robot around the wheels axis. If I understand it correctly, you cannot get any velocity information out of that.
One sensor that is used for approximating the velocity of a robot is the wheel encoders (mentioned a while back in this thread), where you get revolutions/time which is velocity.

psychoul, you are right, you cannot get any velocity information out of a Gyro/Acc combo.
Without at least one encoder you get only basic balance.
Encoder is definitly needed to remove foward/backward drift

Title: Re: Balancing robot for dummies
Post by: jondaeh on Dec 19, 2010, 05:22 pm
@beautifulsmall
A way to make a balancing robot stand still, is to use a state-feedback controller. Basiclly, what you do is to decide what "goals" you want to have for the system, like angle=0, (angle d/dt)=0, (angle-integrated)=0 and motorspeed=0. By measuring or estimating these values(or states) you make a proposional-controller for each of them, and sum up the result from each term and use this as the controlinput for the motors. This is essentially what kas has done when feedbacking the encoder data and the (estimated) derivative for it.

Usually, you want to take base in a mathematical model for the dynamics of the robot when using this approach, as the correct amplificationfactors can be hard to find. With this model you can use pole-placement or LQR-design and decide how you want the robot to react, and then(with a lot of luck) get it that way. If someone is interested, this side gived a good desciption of this methods. I also think there is an example of how to make model of a inverted pendulum(essentially a balancing robot).  

http://wikis.controltheorypro.com/

This method(also called modal-controller) was our plan to use when making the balancingrobot at school. The problem is that we didn't have encoders on the motors, and therefor we tried to assume that the speed of the motors were equal to the input to the motorcontroller(speedservo principial). But as it turned out that the motors had a much bigger time-to-rise than we thought, this wasn't good enough. A solution could have been to use a estimator(as proposed here by psychoul) to estimate the speed of the motors. This again requires a mathematical model for the motors, which we didn't have.

The last update for this robot is that I have shortened is down to make it look more like a "normal" balancing robot, and I have now got it pretty stable using PID controller. Then I know it works :) The plan for this christmas is to update the mathematical model for the shortened version, try to find a how the motors behave and make some sort of an estimator for the motorspeed, to come around the lack of encoder. Can be interesting to see if it will work!

However, when reading your post, I get the impression that what you want the robot to do is to make it a goal to stand still, and then make it "discover" that this can only be achived by making the angle equal zero(or rather equalibrium). To achive this my guess is you will have to use some sort of a neural network or(as you mentioned) fuzzy logic or other adaptable algorithm. This is basiclly something that continuous udates the controller parametres and the equalibrium-angle based on how the system seems to act. This makes it possible to put a weigth on top of the robot, and thereby change the equalibrium angle and the dynamic of the robot, and the controller will automaticlly be tuned to cope with the change. Really cool! The arthour of this youtube video seems to be using a kalmanalgorithm to obtain dynamic equalibrium angle.
http://www.youtube.com/watch?v=dQWATsLa30g
and another video of a inverted pendulum controlled using some sort of adaptive algorithm:
http://www.youtube.com/watch?v=Ci_y14y3DU4
But as I have no experince with adaptive algorithms, what I have said on these few lines is pretty much all I know. Hope someone can jump in and write some complementary lines about this intriguing field :D!

Well, again I managed to do a lot of talk here. Hope someone can find some of it interesting. I have been working on projects like this for quite a time now, and I have made myself a lot of thoughts about the field of system control :P

@kas -> beautifulsmall
Yes, a codensnippet of some fyzzylogic would be great! :D


cheers!
Jon, Norway
Title: Re: Balancing robot for dummies
Post by: kas on Dec 19, 2010, 06:48 pm
@beautifulsmall
@Jon D.
Now, this thread is getting interesting   :P :P
Title: Re: Balancing robot for dummies
Post by: kas on Dec 19, 2010, 09:39 pm
Quote
Hi.

Great guide to balancing robots. Its nice of you to share detailed "how to" info. i especially like your zeroing out imu.

that said, i also have critisizm. Your explanation of the PID regulator is thurough, but faulty. The theory is right, but in your implementation, its all messed up. you forgot a critical part of the regulator.

Let me explain:

P = E, where E is the error, and Kp is the propotional gain.
I = I + E*dt, where dt is time between your samples(looptime).
D = (E - last_E)/dt, which gives you the change with respect to time.

BUT, earlier, you define your looptime as 10ms, so, if your looptime is always 10ms, the above is not nessesary. another BUT, is that if youre looptime exeeds 10ms, the above will work better.

I dont intend to be a knowitall, i'm just trying to help.

Btw, i also used tom pycke's guide to kalman. there i also have a trick that might help. I used serial to log accangle, gyrorate and dt, and then wrote the filter in matlab. there i could tune the filter using the samples(imported as an array of data to matlab). worked great. super fast tuning. you probably dont need to know, but if others use different sensors with different noise, its a really great way to test and tune.

again, thanks for sharing. lots of great info in your tutorial.

Hi danielaaroe, thanks for your comments

Quote
P = E, where E is the error, and Kp is the propotional gain.
I = I + E*dt, where dt is time between your samples(looptime).
D = (E - last_E)/dt, which gives you the change with respect to time.
That the way it should be
I use the actual looptime (lastLoopTime) to feed the Kalman function
for the PID, I opted for speed optimization
Looptime is fixed, so dt is omitted and included in the Ki and Kd values
Pro:
- faster code
Cons:
- changing Loop time implies to modify both Ki and Kd values
- One as to make sure that lastLoopTime does not exceed STD_LOOP_TIME (10ms)

Code: [Select]
void loop() {

 ................................


// *********************** loop timing control **************************
 lastLoopUsefulTime = millis()-loopStartTime;
 if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
 lastLoopTime = millis() - loopStartTime;
 loopStartTime = millis();
}

Within my specific implementation lastLoopUsefulTime is 4ms, so I am safe
Problem could arise at the debugging stage, with several Serial.print() statements within the loop.
Title: Re: Balancing robot for dummies
Post by: kas on Dec 20, 2010, 03:39 pm
Hi beautifulsmall,

I have "off" discussions with Jon D. via PM
He purchased those motors (http://www.pololu.com/catalog/product/1103) (w/o encoders)

Quote
Yes, applying encoders is definitely the easiest and most accurate solution.
If beautifulsmall succeeds with this I'm absolutely interested in it!
The reason I want to make a estimator is mostly to use some of the theory we have learned at school in practice.
I just think it's very fascinating when these things actually works Having both an estimator for motorspeed and
encoders would be really cool, to be able to compare the result! Well, that project will have to wait until next step

I mentioned your ADNS-7550 project
Any possibility to include Jon D. as another beta tester ??  ;)
Title: Re: Balancing robot for dummies
Post by: Paul02 on Dec 23, 2010, 05:01 pm
Thanks for all the great information here!  I'm new to using motor drivers and have a question on how to choose the correct power supply for this application.  I'm using the same 29:1 Pololu gearmotors with a dual motor driver capable of 2.5A continuous and 5A peak output per channel.

My original thought was to use a 12V 5A power supply, but after some forum searching I've read that the motors could draw up to twice the stall current if commanded to switch from full forward to full reverse (for a total of 10A between the two motors).

When picking a power supply for the balancing bot is that 10A the current capacity I need to be accounting for (so then a 12V 10A source?) or since that maximum will probably not be experienced would the 12V 5A work fine?

My other source of consternation is in regards to using a battery to power the unit.  I've found many 12V 3800mah batteries from ebay sellers, but no maximum current is listed for them.  Is it just safe to assume any 3800mah rating means the battery would be able to discharge 10A for 3.8hours?

Thanks to anyone for help with these newbie questions.  I'll edit this with a link to the specific battery / power supply that I'm looking at when I can post a link.
Title: Re: Balancing robot for dummies
Post by: jondaeh on Dec 24, 2010, 12:40 am
@Paul2
Yes, you are right. The biggest load the motors can pull is 5A each. However, as you are not(hopefully) planning to hold them from turning, they will never pull this much more than for an very short moment. Assuming then you are gonna use them in a balancing robot, which weight isn't of to big dimensions.

You don't say what type of battery you are gonne use, Nicd/NiMh or LiPo. I have always used LiPo for my projects, as they deliver very stable voltage as they don't drop so much as they get charged out, and they are quick to recharge. Drawback is that they can not be completely emptied, as they then will be damaged.

I have no experience with NiCd/NiMh batteries, but what you are gonna look for on a LiPo-battery is a rating "C". It says many amperes it is able to deliver, respective to it's capacity. For example, if the battery you are reffering to is rated with 10C, it is able to deliver 3800mah * 10 = 38A continusly. Usually the battery also got a peak-rating,  wich works the same way only that this current can only be delivered for a short amount of time.

If you are new to these kind of batteries it is worth noting that the 2-cell LiPo(2S) is 7.4V and 3-cell(3S) is 11.1V. However, fully charged they measure about respecitve 8.4V and 12.6V, so the last works great on 12V motors.

On my robot a use a battery similar to this one for driving the motors:
http://hobbycity.com/hobbyking/store/uh_viewItem.asp?idProduct=9163
And something like this one to supply the Arduino, Xbee etc.
http://hobbycity.com/hobbyking/store/uh_viewItem.asp?idProduct=9170

The reason I'm using two separate battrypack for driving the motors and the electronics, is to try to avoid noise. The negative pole for both batteries is connected through a 100K resisitor to provide common "ground". Don't know if the motordrivers produces so much noise that this is necessary, though..

Well, good luck Paul2, and merry christmas to you all!


Jon, Norway.
Title: Re: Balancing robot for dummies
Post by: richiereynolds on Dec 24, 2010, 01:53 pm
Folks, hoping for some tips. I've built a balancing robot using an IMU 5DOF, the MD25 motor controller and EMG30s but the motors simply don't seem to respond quickly enough.
I've pretty much used the code being discussed on this thread with a few exceptions -
I'm using an arduino mega so have used the internal ref voltage of 2.56V and the amplified gyro outputs on the IDG500 so my gyro conversion looks like this:

Code: [Select]

int getGyroRate()
{    
   return int(sensorValue[GYR_Y] * 0.781440781);  // in quid/sec:(1024/360)/1024 * 2.56/0.0091)
}


I'm using I2C for the motor controller which needs a 0 to 255 where 128=stopped so to get my drive value I do:
Code: [Select]

int updatePid(int targetPosition, int currentPosition)  
{  
   int error = targetPosition - currentPosition;
   pTerm = Kp * error;
   integrated_error += error;
   iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
   dTerm = Kd * (error - last_error);
   last_error = error;
   
   int x = -constrain(K*(pTerm + iTerm + dTerm), -255, 255);
   x /= 2;
   x += 128;  
   return x;
}


When calibrating my sensors and getting readings I disregarded any outliers:
Code: [Select]

int compare (const void * a, const void * b)
{
   return ( *(int*)a - *(int*)b );
}

void calibrateSensors()
{
   int allVals[50];  
   long v;
   
   for(int n=0; n<3; ++n)
   {
       v = 0;
       for(int i=0; i<50; ++i)      
       {
           allVals[i] = readSensor(n);          
       }
       qsort(allVals, 50, sizeof(int), compare);
       
       for(int i=5; i<45; ++i)
       {
           v+= allVals[i];
       }
       sensorZero[n] = v/40;
   }
   sensorZero[ACC_Z] -= 137;
}

void updateSensors()
{
   int allVals[5];
   long v;
   for(int n=0; n<3; ++n)
   {
       v = 0;
       for(int i=0; i<5; ++i)      
       {
           allVals[i] = readSensor(n);
       }
       qsort(allVals, 5, sizeof(int), compare);
       
       for(int i=1; i<4; ++i)
       {
           v+= allVals[i];
       }
       sensorValue[n] = v/3 - sensorZero[n];
   }
}


I also timed my loop over 1000 iterations and found it took about 3.5ms per iteration so I've tried setting the STD_LOOP_TIME to 5 but it didn't help.

My angles look pretty good and if I print out the time, angle and drive as they change they look ok though the time is slowed down by the printing.
However, if I let the robot go from vertical I can see it's almost beyond the point of no return before the motors kick into life.
If I stop it falling too far on each side by hand I can see that the motors are doing the right thing, just not quick enough.
I've mainly just tried increasing the P val for pid so far, I and D are 0, though I've tinkered there too with no joy.

I don't know much about I2C, could it be just to slow for this task? Surely the EMG30s are up to it?

Appreciate any advice!
Title: Re: Balancing robot for dummies
Post by: jondaeh on Dec 26, 2010, 02:59 pm
@richiereynolds
Quote
I don't know much about I2C, could it be just to slow for this task? Surely the EMG30s are up to it?

I'm sure I2C is fast enough, it's used widely in gyros and accelerometers with success, so it should work the other way(as output, that is) too. The motors seem pretty similar to pololu motors, and looking at the specifications they should be good for the purpose.

Quote
However, if I let the robot go from vertical I can see it's almost beyond the point of no return before the motors kick into life.
If I stop it falling too far on each side by hand I can see that the motors are doing the right thing, just not quick enough.

I have noticed that these motors typically have some delay before they actually starts turning. That means they doesn't start when you apply "1" to the motordriver, but not until you apply "5" or something. This implies that the robot has to "fall" longer until a standalone P-val will get a big enough value to start turning the motors. I solved this by making a offset, so that when the PID-controller puts out "1", the motors will start turning. This is the code I use for driving the motors:

Code: [Select]
void MotorDriver::setMotor(signed int setting)
{
  if(setting > 0) setting += 4;//Increase for bigger offset on the + side
  else if(setting < 0) setting -= 4;//Increase for bigger offset on the - side
 
  if(setting > 100) setting = 100;
  else if (setting < -100) setting = -100;
 
  setting = setting * 1.27;
  analogWrite(5, -setting + 127);
  analogWrite(6,  setting + 127);
  analogWrite(9,  setting + 127);
  analogWrite(10,-setting + 127);
}


Try experiment with different offsets, the weight of the robot and size of the wheels can make the dynamic differ.  

The argument for this function should here be -100 -> +100, but you get an idea for what I'm doing :)

@All
I have now hosted my balancerobot code, if anyone is interested :)
http://code.google.com/p/veltobot/


Jon, Norway

Title: Re: Balancing robot for dummies
Post by: richiereynolds on Dec 27, 2010, 11:01 pm
Thanks! I'll give those ideas a go and let you know how I get on.
Title: Re: Balancing robot for dummies
Post by: beautifulsmall on Dec 27, 2010, 11:14 pm
Kas, Jon-D, theres a good few links there i need to look up, thanks. as for fuzzy i have not tried it but there is a pic app note i will try to post the link for.
RE ADNS-7550 laser optical mouse encoder as a velocity encoder:
so far i have placed the encoder over a flat belt which is connected to a motor which has a heds slotted disk encoder fitted  therefore i get quadrature encoder data to compare with the optical .
1. at constant velocity the two encoders give an average velocity which is comparable within roughly 10%.
2. the optical encoder gives an erratic reading , over and under the heds encoder.
3. for zero velocity both read zero
4. for very low velocity, 1mm/sec the optical reads zero !
5. for absolute position , ie step changes, the optical very quickily has a large error. this can be tested with a pc mouse, just try to move it repeatedly back to a fixed position and see where the arrow ends up!

I emailed AVAGO with these findings and asked if the sensor has a high pass filter, they said they dont spec low speed, also the absolute accuracy is only 20-30% ! shock. also (area of image is < 2mmsq)
despite this im not giving up hope.
it is possible to read the pixel image data, if a pattern is used where the absolute dimensions are known then surely a more accurate calculation can be done. and don't call me shirley.
I asked if the ADNS-9500 super gaming sensor was any better but no mention was made in their reply, and you cant get one for love or money, same family anyway but it does need a firmware download so theres hope for a special version ?
It may be that my 7550  just didnt have the right surface but one register reports surface quality , i never got better than 75%, i tried
black htd M3 belt- raw - 0%
lightly sprayed with gold xmas metallic - 75%
any colored card 50%
aluminium foil - rubbish
embossed plastic metallized sheet - 30 %
white card with very fine black fibers - 30 %  
I still need to test a lumpy dull surface. Ill try it on my brothers wife .

RE motor drivers, check that acceleration control is turned off, current limit, all the saftey features!, im just building with a new one, vn??
merry xmas everyone, :) :)

Title: Re: Balancing robot for dummies
Post by: retrolefty on Dec 27, 2010, 11:26 pm
Quote
I still need to test a lumpy dull surface. Ill try it on my brothers wife


This is either a great insiders joke or a completely WTF comment.  ;D

Lefty
Title: Re: Balancing robot for dummies
Post by: kas on Dec 28, 2010, 12:42 pm
Quote
Kas, Jon-D, theres a good few links there i need to look up, thanks. as for fuzzy i have not tried it but there is a pic app note i will try to post the link for.
RE ADNS-7550 laser optical mouse encoder as a velocity encoder:
so far i have placed the encoder over a flat belt which is connected to a motor which has a heds slotted disk encoder fitted  therefore i get quadrature encoder data to compare with the optical .
1. at constant velocity the two encoders give an average velocity which is comparable within roughly 10%.
2. the optical encoder gives an erratic reading , over and under the heds encoder.
3. for zero velocity both read zero
4. for very low velocity, 1mm/sec the optical reads zero !
5. for absolute position , ie step changes, the optical very quickily has a large error. this can be tested with a pc mouse, just try to move it repeatedly back to a fixed position and see where the arrow ends up!

I emailed AVAGO with these findings and asked if the sensor has a high pass filter, they said they dont spec low speed, also the absolute accuracy is only 20-30% ! shock. also (area of image is < 2mmsq)
despite this im not giving up hope.
it is possible to read the pixel image data, if a pattern is used where the absolute dimensions are known then surely a more accurate calculation can be done. and don't call me shirley.
I asked if the ADNS-9500 super gaming sensor was any better but no mention was made in their reply, and you cant get one for love or money, same family anyway but it does need a firmware download so theres hope for a special version ?
It may be that my 7550  just didnt have the right surface but one register reports surface quality , i never got better than 75%, i tried
black htd M3 belt- raw - 0%
lightly sprayed with gold xmas metallic - 75%
any colored card 50%
aluminium foil - rubbish
embossed plastic metallized sheet - 30 %
white card with very fine black fibers - 30 %  
I still need to test a lumpy dull surface. Ill try it on my brothers wife .

RE motor drivers, check that acceleration control is turned off, current limit, all the saftey features!, im just building with a new one, vn??
merry xmas everyone,  

Hi beautifulsmall, I am impressed  :o
I suspect you may end up working for AVAGO  :)


I am preparing "Balancing robot for dummies part seven" (wireless Xbee control +  KasBot V 3.0) and expect to be ready within a week
Title: Re: Balancing robot for dummies
Post by: Villa on Dec 29, 2010, 03:35 pm
Hi everybody,

This is my first post at Arduino forum and I'd like to say thank you for creating such a great topic.

Just like you, I am interested in two wheel balancing robots. I have built my own one. My robot's construction is similar to this one:
http://www.youtube.com/watch?v=_TXfXoKyMzc&feature=player_embedded
but mine is bigger. I am sorry I cannot provide you with a photo of it right now. I will do so as soon as I can.

When trying to balance the robot, I have a couple of questions as follow:

1. Since the two motors are not perfectly identical, there should be two more PID control loops (apart from the main balance PID loop), one for each motor, to guarantee that the real motor's velocity will strictly follow the set one. In my case, this is actually a problem because when I apply an equal voltage to the two motors, they rotate with different speeds. Especially when one wheel hits an obstacle, say a crack on the ground, and cannot cross it, the robot will rotate around the wheel (in stead of stopping or moving forward). But I didn't see such a problem mentioned before at this thread.

2. Currently I am using the cascaded PID control approach to balance the robot. This approach has been used successfully by Brian Kuschak with his bk-bot:
http://www.bkinnovation.com/bkbot/
My robot can barely balance, in the sense that it doesn't fall, but it oscillates around the equilibrium point with a frequency of 1 to 2 Hz and the oscillation's amplitude is pretty large (the wheel moves back and forth with a travel distance of ~3 inches). I have spent days tuning the PID parameters to reduce this oscillation but so far I have had no improvements. When being pushed, the robot can keep its balance, but it oscillates even more wildly. Please look at this picture to see how my robot responds to a push:
(http://i256.photobucket.com/albums/hh191/kgjEhi78238a9gHw/velocity_response-1.png)

How do you guys think about this? Is this resulted from an improper set of control parameters? If you know a correct procedure for tuning the PID parameters, please show me the way ^^!

3. The third question is about the wheel size. From my observation, it seems that with bigger wheels it is easier to balance, am I correct? Currently my wheels are made from plastic. They are hard and small (their diameter is about 3 inches, which is quite small because my robot is 1.2 metres in height). Will rubber wheels with bigger diameter be better?

Thank you for reading my questions. Any answer would be very appreciated.

I wish you a happy new year ^^

Villa
Title: Re: Balancing robot for dummies
Post by: kas on Dec 31, 2010, 11:33 am
Hi Villa, welcome aboard

Please post a photo of the beast as soon as you can

Quote
1. Since the two motors are not perfectly identical, there should be two more PID control loops (apart from the main balance PID loop), one for each motor, to guarantee that the real motor's velocity will strictly follow the set one. In my case, this is actually a problem because when I apply an equal voltage to the two motors, they rotate with different speeds. Especially when one wheel hits an obstacle, say a crack on the ground, and cannot cross it, the robot will rotate around the wheel (in stead of stopping or moving forward). But I didn't see such a problem mentioned before at this thread.
Each motor has a different PWM/speed response. I trim left PWM motor within the code and make adjustments while driving the bot using the RC controller' joystick.
You are right, we need more sophistiated control, a single PID loop doesn't cope with the job.
Also those DC motors don't run well at low speed, below PWM < 20 (no torque)
see DC motor with PID (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0)

I am impressed by this control scheme shown on the above link (http://www.bkinnovation.com/bkbot/)
(http://i.imgur.com/3PIA6.png)
I will contact Brian and ask him to stop by

Quote
2. Currently I am using the cascaded PID control approach to balance the robot. This approach has been used successfully by Brian Kuschak with his bk-bot:
http://www.bkinnovation.com/bkbot/
My robot can barely balance, in the sense that it doesn't fall, but it oscillates around the equilibrium point with a frequency of 1 to 2 Hz and the oscillation's amplitude is pretty large (the wheel moves back and forth with a travel distance of ~3 inches). I have spent days tuning the PID parameters to reduce this oscillation but so far I have had no improvements. When being pushed, the robot can keep its balance, but it oscillates even more wildly. Please look at this picture to see how my robot responds to a push:
How do you guys think about this? Is this resulted from an improper set of control parameters? If you know a correct procedure for tuning the PID parameters, please show me the way ^^!
Looking at the above diagram, my first idea would be to increase abs(Kd). You need more damping (see post #32 (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/30)). Others will jump with additional ideas.
Also try changing the sign of Kd (mine is negative)

Quote
3. The third question is about the wheel size. From my observation, it seems that with bigger wheels it is easier to balance, am I correct? Currently my wheels are made from plastic. They are hard and small (their diameter is about 3 inches, which is quite small because my robot is 1.2 metres in height). Will rubber wheels with bigger diameter be better?
I went from 80mm to 90mm wheels, I definitly noticed an improvement due to higher speed and larger contact surface with the ground.
Motors should be strong enough to cope with the additional torque.

Title: Re: Balancing robot for dummies
Post by: kas on Dec 31, 2010, 07:28 pm
Quote
From post #148 (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/135)
Quote
How about you. It seems you are interested about control as well. Any other interesting projects or something?

...
I started a new exciting project also based on control/balance.
I wont talk about it now to avoid polluting this thread

Here is the new project  :)

(http://i.imgur.com/lmPQ1.jpg)
@Ro-Bot-X you know this one don't you ??  ;)

The objective is to free henpecked husbands from boring domestic duties  ;D ;D ;D
I fact I may also develop a balancing vacuum cleaner  ::)


             [size=18]**  Happy New Year to everybody  **[/size]


Title: Re: Balancing robot for dummies
Post by: niebing1987 on Jan 01, 2011, 01:06 pm
Hi, kas. Happy New Year to you!
I am doing my own balancing robot. However, there is something wrong with the fusing.
My filtered angle seems to lag the real angle for about several seconds. My accelerometer is ADXL202, and the gyro is ENC03, the Kalman filtering code which I use is from this thread .Here is the imagine on my filtering.
ftp://http://user.qzone.qq.com/513837087?ADUIN=513837087&ADSESSION=1293883100&ADTAG=CLIENT.QQ.3067_MyTip.0&ptlang=2052
Could you give some suggestions on it? Thank you!
Code: [Select]
void PIDAnglePosition(INT32S* const pGivenSpeed1, INT32S* const pGivenSpeed2, INT32S CarSpeed)
{
     INT32S ADData = 0,Acc = 0,i;
     FP32 MeaAngle,MeaAngleDot;                  FP32 AngleError,temp;
     

     Acc = MeasureAcc() + ACC_OFFSET;
     MeaAngle = asin((FP32)Acc/GRAVITY_G);                  //Measure angle by the accelerometer
     temp = MeaAngle*1000;            //translate the float into integer by multiplying 1000,then send it to PC by serialport
     SendData[4] = (INT32S)temp%1000;
     SendData[3] = (INT32S)(temp/1000);
           
     ADData = ReadTLC4541();                  //measure the anglespeed by the gyro  5000.0*(ADData-RefAD)/65536.0/0.67/K;
     MeaAngleDot = 0.001995*(RefAD-ADData) / K;
     temp = MeaAngleDot*1000;
     SendData[6] = (INT32S)temp%1000;
     SendData[5] = (INT32S)(temp/1000);
     
     kalmanCalculate(MeaAngle,MeaAngleDot);

     temp = x_angle*1000;
     SendData[8] = (INT32S)temp%1000;
     SendData[7] = (INT32S)(temp/1000);
     
     AngleError = - x_angle;
     Position += CarSpeed;                  
      *pGivenSpeed2 = *pGivenSpeed1 = AngleKp*AngleError + AngleKi*FiltAngleDot
                                                 + PositionKp*Position + PositionKi*CarSpeed;      
}

Title: Re: Balancing robot for dummies
Post by: SnakeLT on Jan 01, 2011, 03:32 pm
Hello guys,
At first I want to say a word about bk-bot control algorithm, it will not  work correctly, main error is that balance pid output is torgue not speed, so...
Second do not use kalman filter as main angle output, it is a little laggy and not enough accurate. Use only gyro and incremental encoders.
Ok you will say gyro have not angle reference and integration is inaccurate. Yes it is, but here is a trick, in long term equilibrium angle is always 0deg, so you only need to know short term angle. So in long term angle, program artifically slowly averaging angle to 0deg.

Title: Re: Balancing robot for dummies
Post by: SnakeLT on Jan 01, 2011, 03:32 pm
Here is my robot:
http://www.youtube.com/watch?v=y-q3zOc6KVk&feature=player_embedded
http://img594.imageshack.us/f/86546394.jpg/
http://img819.imageshack.us/i/56664166.jpg/
http://img221.imageshack.us/img221/280/bbcontrol.png
Title: Re: Balancing robot for dummies
Post by: GuzZzt on Jan 01, 2011, 06:01 pm
SnakeLT: Very impressive robot, can you give us any details on the algorithms you use? Normal PID loop as anyone else here or anything fancier ;)?
Title: Re: Balancing robot for dummies
Post by: SnakeLT on Jan 01, 2011, 06:23 pm
I'm using interactive (series) PID.
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Jan 01, 2011, 06:33 pm
Happy New Year!

@kas: yes, that is my long term goal, but I fear it's way too hard for me to do, as I lack solid programming skills and have no time to learn and experiment

Lately I've been swamped by work, but I started some other projects that take me away from my poor balancing bot. I did not built a proper frame for my bot, I've been trying to see if a regular low profile robot can do balancing. I've come to the conclusion that large wheels are important, high COG improves balancing, high speed is required for fast corrections. That being said, my poor low height robot with small wheels and not enough speed has trouble balancing even for a split second.

So keep it up guys, I'll do some more work on my bot in a few weeks when I'll be off work.

Cheers!
Title: Re: Balancing robot for dummies
Post by: Villa on Jan 01, 2011, 06:39 pm
@kas: Thank you for your answers. I will try your suggestions when I have access to my robot  :)

@SnakeLT: Could you explain more about this:
Quote
At first I want to say a word about bk-bot control algorithm, it will not  work correctly, main error is that balance pid output is torgue not speed, so...

Why is it that the balance pid output must be torque, not speed?

Thank you,

Villa
Title: Re: Balancing robot for dummies
Post by: SnakeLT on Jan 01, 2011, 06:58 pm
It does not must be torgue, but in this case it is. At output you got torgue and you interpret it like speed. Look at my algorithm, and dont use motor speed pid.
Title: Re: Balancing robot for dummies
Post by: kas on Jan 01, 2011, 06:59 pm
Quote
Hi, kas. Happy New Year to you!
I am doing my own balancing robot. However, there is something wrong with the fusing.
My filtered angle seems to lag the real angle for about several seconds. My accelerometer is ADXL202, and the gyro is ENC03, the Kalman filtering code which I use is from this thread .Here is the imagine on my filtering.
http://user.qzone.qq.com/513837087?ADUIN=513837087&ADSESSION=1293883100&ADTAG=CL...
Could you give some suggestions on it? Thank you!

Hi niebing, nice to see you here
I noticed some asian characters (now gone) in your code, out of curiosity, where are you based ??

I can't access the Web address you indicated, from my PC.
Seems you use only one ACC and asin() vs atan().
Your code is too different from Kasbot, sorry, I can't help  :(
Please let us have a photo of your bot

Title: Re: Balancing robot for dummies
Post by: niebing1987 on Jan 02, 2011, 02:45 am
I am from China.
Quote
Seems you use only one ACC and asin() vs atan().

I just used one axis to measure the angle by accelerometer. I want to know is it right?
About my robot, i do not have built up it,  i am only doing the experiment on the fusion.So i do not have the photo of my robot, Sorry :(.
I changed my link on the photo,could you have a try?
the black line is the angle measured by the accelerometer. the read one is angle speed , the blue one is filtered angle
http://niebing1987.tuzhan.com/photo/f1998db0d3002b80.html
Title: Re: Balancing robot for dummies
Post by: Villa on Jan 02, 2011, 04:59 am
Quote
It does not must be torgue, but in this case it is. At output you got torgue and you interpret it like speed. Look at my algorithm, and dont use motor speed pid.


Yeah, I understand what you mean, but my question is on what basis/theory did you make that conclusion? Could you please give me a more thorough explanation?

Another thing I wonder is about the sample time of the PID loops? How fast the driving loop, the balance loop and the heading loop should be?

Thanks,

Villa
Title: Re: Balancing robot for dummies
Post by: kas on Jan 02, 2011, 11:05 am
Hi SnakeLT, thanks for joining the group
I understand you are from Lituania (cold is it??)  ;)
As for most of us here, English is not your native language.
Please take your time, work at your own speed, and produce structured technical information

Your bot is really cute and agile, the frame is... a CD box  :)
Which development platform are you using ??
(http://i.imgur.com/YSbaX.png)
You do not use accelerators and the reason you gave...
Quote
in long term equilibrium angle is always 0deg, so you only need to know short term angle. So in long term angle, program artifically slowly averaging angle to 0deg.
...is pretty smart 8-)

Also I don't see motor velocity in your diagram. Is it computed in the "Driving PID" bloc ??
Please let us have additional info on your control strategy, together with source code or pseudo code

I now realize that, as Villa mentioned earlier, a single PID loop will never produce a completly still robot
Also, a single encoder is not definitly enough to keep a fixed heading


Title: Re: Balancing robot for dummies
Post by: kas on Jan 02, 2011, 01:37 pm
@niebing1987

You can definitly aquire angle with only one ACC
The angle is small and some folks assume sin(x) = x   (in radians)
Now... the ADXL202 is a dual ACC, why don't you use the 2 axis for a more linear mesurement ??  :)

Code: [Select]
     Acc = MeasureAcc() + ACC_OFFSET;
     MeaAngle = asin((FP32)Acc/GRAVITY_G);      //Measure angle by the accelerometer
Can I assume ACC_OFFSET is sensorZero[] ?
Is GRAVITY_G the gravity offset ? (  (ADC value "upward" - ADC value "downward")/2  )
Why is this value divided rather than substracted ?
Are MeaAngle and MeaAngleDot refering the same units for angle (deg, rad, quids...) ?


Code: [Select]
     ADData = ReadTLC4541();  //measure the anglespeed by the gyro 5000.0*(ADData-RefAD)/65536.0/0.67/K;
     MeaAngleDot = 0.001995*(RefAD-ADData) / K;
You use a 16 bit serial ADC to access the Murata gyro
Try changing the sign of MeaAngleDot

With so few information, those are only shots in the dark
What microcontroler are you using ? is it fast enough ?

Looking at the diagrams, ACC and Gyro are in phase
(gyro is zero when movement is inverted)
Your Kalman graph is just dead and may need different parameters
see danielaaroe post #157 (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/150)

Good luck in your quest  ;), keep us aware


Title: Re: Balancing robot for dummies
Post by: SnakeLT on Jan 02, 2011, 02:58 pm
Villa, I'm not physicist and can't explain it.  In balancing robot case torgue is something like robot heigth*gravity*sin(angle), balance pid directly deals with angle. In my robot all PID's run at 100Hz.

kas, yes I'm from Lithuania, not so cold -4degC, but alot of snow.
I'm using GCC for avr, main procesor xmega. Yes speed derived from position (D term).

Angle calculation:
Code: [Select]
volatile float gOffset = 0;
float KOF = 0.004; //0.005
volatile float G_Angle = 0;
float TimeInterval = 0.01;

void GetGyroData(int GyroRaw)
{
 float GyroSpeed;
 gOffset = KOF * GyroRaw + (1-KOF) * gOffset;
 GyroSpeed = GyroRaw - gOffset;
 G_Angle += gyroSpeed*TimeInterval;
}
Title: Re: Balancing robot for dummies
Post by: niebing1987 on Jan 02, 2011, 03:08 pm
Hi,kas
Quote
Now... the ADXL202 is a dual ACC, why don't you use the 2 axis for a more linear mesurement ??  

When I firstly read the details about the robot, they just used one axis and asin, so i  :(.....but i will make the change.
Quote
Can I assume ACC_OFFSET is sensorZero[] ?
Is GRAVITY_G the gravity offset ? (  (ADC value "upward" - ADC value "downward")/2  )
Why is this value divided rather than substracted ?
Are MeaAngle and MeaAngleDot refering the same units for angle (deg, rad, quids...) ?

yes,ACC_OFFSET is the sensorZero
however, GRAVITY_G is not the gravity offset, it is the ADC value when the axis is paralleled to the gravity.
Quote
What microcontroler are you using ? is it fast enough ?
Looking at the diagrams, ACC and Gyro are in phase
(gyro is zero when movement is inverted)
Your Kalman graph is just dead and may need different parameters

my microcontroller is LPC1114, 32bit, colock 48MHz, it should be fast enough.
For my poor english, i do not understand clearly ACC and Gyro is in phase, is it right like this?
Title: Re: Balancing robot for dummies
Post by: richiereynolds on Jan 02, 2011, 07:34 pm
Hopefully a quick one folks, in the v2 code for the serial GUI it has this function:

Code: [Select]

int getGyroRate()
{                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
 return int((sensorValue[GYR_Y] * 0.88888888889)*-1);
}


Where does the 0.88888888889 constant come from? it doesn't seem to match any ref voltage etc.

Thanks!
Title: Re: Balancing robot for dummies
Post by: X-firm on Jan 02, 2011, 09:49 pm
@ richiereynolds

I'm using a nother GYR called LPR510AL  (http://www.pololu.com/catalog/product/1266) and that one has a sensitivity of 10 mV/(°/s) at ±100°/s.

It looks like I have made a miss calculation where it should have been:  

// ARef=3.3V, Gyro sensitivity=10mV/(deg/sec)
// in quid/sec:(1024/360)/1024 * 3.3/0.010)
return int((sensorValue[GYR_Y] * 0,9166679)*-1);

Measurement range:       ±100°/s and ±400°/s
Sensitivity:       10 mV/(°/s) and 2.5 mV/(°/s)

That could explain some errors I been having..

Sorry  :(

I have rebuild my robot lighter but I haven't fixed the electronics on It yet hopefully I will get It done next week.

I have also ported the GUI to the .net platform but I'm not satisfied with the serial communication. Is there any one how has a good idea how to send the amount of data I'm doing in a better way.

Like it is now it will not be able to send it and still be able to keep the fixed loop time at 10ms. I solve it by only sending data to the GUI each 5 loops..

Title: Re: Balancing robot for dummies
Post by: X-firm on Jan 02, 2011, 10:17 pm
The functions looks like this to day:
Code: [Select]

int skipOut=0;

void serialOut_GUI() {  
 
 if(skipOut++>=updateRate) {                                                        
   skipOut = 0;
   //Filtered and unfiltered angle
   Serial.print(ACC_angle, DEC);  Serial.print(",");
   Serial.print(actAngle, DEC);   Serial.print(",");
   
   //Raw sensor data
   Serial.print(sensorValue[ACC_X], DEC);   Serial.print(",");
   Serial.print(sensorValue[ACC_Z], DEC);   Serial.print(",");
   Serial.print(sensorValue[GYR_Y], DEC);   Serial.print(",");
 
   Serial.print(pTerm, DEC);      Serial.print(",");
   Serial.print(iTerm, DEC);      Serial.print(",");
   Serial.print(dTerm, DEC);      Serial.print(",");
   Serial.print(drive, DEC);      Serial.print(",");
   Serial.print(error, DEC);      Serial.print(",");
   Serial.print(setPoint, DEC);   Serial.print(",");
   
   //PID Parameters
   Serial.print(K, DEC);    Serial.print(",");
   Serial.print(Kp, DEC);   Serial.print(",");
   Serial.print(Ki, DEC);   Serial.print(",");
   Serial.print(Kd, DEC);   Serial.print(",");
   
   //loop
   Serial.print(STD_LOOP_TIME, DEC);        Serial.print(",");
   Serial.print(lastLoopUsefulTime, DEC);   Serial.print(",");
   Serial.print(lastLoopTime, DEC);         Serial.print(",");
   Serial.print(updateRate, DEC);           Serial.print(",");
   
   Serial.print(motorOffsetL, DEC);         Serial.print(",");
   Serial.print(motorOffsetR, DEC);         Serial.print(",");  
   
   Serial.print(pTerm_Wheel, DEC);     Serial.print(",");
   Serial.print(dTerm_Wheel, DEC);     Serial.print(",");
   Serial.print(Kp_Wheel, DEC);        Serial.print(",");  
   Serial.print(Kd_Wheel, DEC);        Serial.print(",");    
   Serial.print("\n");
 }
}

union u_tag {
 byte b[4];
 float fval;
} u;

int skipIn=0;

void serialIn_GUI(){
 
 if(skipIn++>=updateRate) {                                                        
   skipIn = 0;
   byte id;
   
   if(Serial.available() > 0)
   {              
     char param = Serial.read();
     //Serial.println("Have bytes");
     delay(10);
     byte inByte[Serial.available()];
     if(Serial.read() == SPLIT){
       if(Serial.available() >= 4){
         u.b[3] = Serial.read();
         u.b[2] = Serial.read();
         u.b[1] = Serial.read();
         u.b[0] = Serial.read();
         Serial.flush();
         
         switch (param) {
           case 'p':
             Kp = int(u.fval);
             break;
           case 'i':
             Ki = int(u.fval);
             break;
           case 'd':
             Kd = int(u.fval);
             break;
           case 'k':
             K = u.fval;
             break;
           case 's':
             setPoint = int(u.fval);
             break;
           case 'u':
             updateRate = int(u.fval);
             break;
           case 'l':
             motorOffsetL = u.fval;
             break;
           case 'r':
             motorOffsetR = u.fval;
             break;
           case 'q':
             Kp_Wheel = int(u.fval);
             break;
           case 'e':
             serial_type = int(u.fval);
             break;
           case 'w':
             Kd_Wheel = int(u.fval);
         }
       }
     }
     
   }
 }
}



It's a realy long line that are beaning send to the GUI maybe it would be better to split it and sending it like bytes like I do when I send data to the Arduino..
Title: Re: Balancing robot for dummies
Post by: richiereynolds on Jan 03, 2011, 12:04 am
Ah, excellent Patrik, thanks, that's that sorted.

My robot's actually working a bit better now, balancing, kind of ...

Going to work on the encoder parts now.
I also put a couple of "emergency wheels" on the bottom that swing down and are also supposed to hold it steady while it does zero calibration at the start, then swing up out of the way to let it balance. Only problem is, the servos only have the torque to hold the robot up when it's near vertical!

Oh well, back to the drawing board ...
Title: Re: Balancing robot for dummies
Post by: richiereynolds on Jan 03, 2011, 12:11 am
On the sending data to the gui question, would it be any quicker to put the strings into a buffer using sprintf and then send it in one Serial.println?
Other than that, I can only think of usng a fixed format that the GUI understands and sending bytes as you say.
Title: Re: Balancing robot for dummies
Post by: Villa on Jan 03, 2011, 05:07 am
@SnakeLT: Its ok. Thank you for your answer. Your robot is really awesome! ^^

@Patrik: I think that you shouldn't perform the data acquisition with a sample time of 10 (ms) in Windows (.NET), because Windows is not a real-time OS and 10 (ms) seems to be a little bit too fast.

@richiereynolds: When sending data to the GUI, You should encode your data using numbers in stead of characters. That way you will save a lot of bandwidth as well as processing time.
Title: Re: Balancing robot for dummies
Post by: niebing1987 on Jan 03, 2011, 05:23 am
kas,thanks for your suggestion.it is the parmeter! After modifying them, the effect of kalmanfilter is much better.
Quote
http://niebing1987.tuzhan.com/photo/09ea592eba247adc.html

;D ;D ;D ;D ;D
Title: Re: Balancing robot for dummies
Post by: kas on Jan 03, 2011, 09:08 am
Quote
I have also ported the GUI to the .net platform but I'm not satisfied with the serial communication. Is there any one how has a good idea how to send the amount of data I'm doing in a better way

Hi Patrik
My first idea is to increase data speed transfer to 115200 bps
I went on your blog http://www.x-firm.com/?p=246
and checked BalancingBotGUI v1.2

- In BalancingBotGUI_v_1_2.pde,
Code: [Select]
 Version log:

   v1.2  ...
       Changed the serial speed to 115200 kb/s


- in serial_screen.pde,
Code: [Select]
void setupSerial()
{
 ...
 myPort = new Serial(this, portName, 19200);
 ...
}

I am confused, are you still transmitting @19200 bps ??  :-?
Please clarify

Title: Re: Balancing robot for dummies
Post by: X-firm on Jan 03, 2011, 09:30 pm
@kas

I have bean running it on 19200bps I tried with 115200 but I went back to 19200 and just sending data each 5 loops..

Is there any downside with using 115200?

What I saw when sending data each loop the arduino could not hold the loop time any more... But when I send it each 5 loops it seams to be ok running around 7-8ms before delay,,,

But maybe I'm fueling my self and each 5 loops the fixed loop time is of and maybe that will give faults in the regulation of the robot..

I want to find a way to be able to send the amount of data I'm sending now in each loop with out going over 10ms.. But still maybe only send It each 5 loops because I don't think the GUI will be able to read it faster..  

I have a idea of sending parts of the data under five loops and then start over like.
Code: [Select]

switch (loop_count){
 case 1:
    //send PID values
 case 2:
    //send PID parameter values
    //K value as float
    //[id byte][split][value][value][value][value][CR]
    //Kp value as int
    //[id byte][split][value][value][CR]
 case 3:
    //send Sensor values
    //send Angel values
 case 4:
    //send Torque values
 case 5:
    //send Settings values
 defualt:
   loop_count = 0;
}
loop_count++;


What do you think about that? And in your opinion what baud rate should we use if possible?
Title: Re: Balancing robot for dummies
Post by: kas on Jan 04, 2011, 10:53 am
@niebing1987

Quote
For my poor english, i do not understand clearly ACC and Gyro is in phase,

My English is not good either  ;)
I mean that the Gyro signal (angle variation, deg/s) can be seen as the first derivative of the ACC signal (deg)
Gyro signal should be zero when ACC signal is horizontal (dX/dt=0)
On the first diagram the red curve cross the zero axis when the black curve reaches a max or a min.
(http://i.imgur.com/G22bW.png)

This is also noticiable on the left side of your second diagram.
(http://i.imgur.com/JYKlr.png)

Kalman parameters are specific to sensors type (noise)
Now your filtered angle is much better, but too much filtered  ;)



@Patrik

Quote
What do you think about that? And in your opinion what baud rate should we use if possible?

I will experiment and be back by the end of the week
Title: Re: Balancing robot for dummies
Post by: niebing1987 on Jan 04, 2011, 01:43 pm
@kas
Quote
I mean that the Gyro signal (angle variation, deg/s) can be seen as the first derivative of the ACC signal (deg)
Gyro signal should be zero when ACC signal is horizontal (dX/dt=0)
On the first diagram the red curve cross the zero axis when the black curve reaches a max or a min.


in my opinion, if the gyro signal is not zero, which means  the Angle wil go on changing, the ACC signal will not be the max or min.
And i wil go on modifying the parameter to improve my kalmanfitering.
Title: Re: Balancing robot for dummies
Post by: retrolefty on Jan 04, 2011, 03:02 pm
I'm curious, would a indication of excellent control be the ability for the balancing robot to be able to 'balance' at any desired angle, rather then just at true vertical? Has anyone been able to do that or have witnessed that?

Lefty
Title: Re: Balancing robot for dummies
Post by: Villa on Jan 05, 2011, 04:28 am
Quote
I'm curious, would a indication of excellent control be the ability for the balancing robot to be able to 'balance' at any desired angle, rather then just at true vertical? Has anyone been able to do that or have witnessed that?

Lefty


I have seen an inverted pendulum which is capable of doing what you described (An inverted pendulum is similar to a balancing robot).

I think for the robot to move back to the balance angle from any initial angle, the controller must be a non-linear controller, i.e. Fuzzy Logic Controller or Adaptive Controller. A linear controller (like PID) will not do the job because with a broad range of angle's value, the robot can no longer be considered linear.
Title: Re: Balancing robot for dummies
Post by: kas on Jan 05, 2011, 10:52 am
Quote
Villa, I'm not physicist and can't explain it.  In balancing robot case torgue is something like robot heigth*gravity*sin(angle), balance pid directly deals with angle. In my robot all PID's run at 100Hz.

kas, yes I'm from Lithuania, not so cold -4degC, but alot of snow.
I'm using GCC for avr, main procesor xmega. Yes speed derived from position (D term).

Angle calculation:
Code: [Select]
volatile float gOffset = 0;
float KOF = 0.004; //0.005
volatile float G_Angle = 0;
float TimeInterval = 0.01;

void GetGyroData(int GyroRaw)
{
 float GyroSpeed;
 gOffset = KOF * GyroRaw + (1-KOF) * gOffset;
 GyroSpeed = GyroRaw - gOffset;
 G_Angle += gyroSpeed*TimeInterval;
}

Hi SnakeLT
Thanks for the info
Would you mind posting the complete source code  :)
Title: Re: Balancing robot for dummies
Post by: bryanpearson on Jan 05, 2011, 07:12 pm
Hello All. First, thank you so much for all of the valuable information. It has helped greatly with my balance-bot project. All the replies are amazing.

Question: I'm using Patrik's v1.2 code, with the Processing 'Balance_bot_gui' code (Thank you Patrik, it rocks.) All is displaying well, my bot is balancing... sort of, but I have a question about the output.

When I look at the error displayed on the Processing GUI, it seems delayed. By about 1-2 seconds. If I hold the bot at-error, and error on the screen displays -80, then I quickly whip it over to where it should display +80, it takes about 1.5-2 seconds for the error on the GUI output to update to that error. (Hope that makes sense.)

Any tips? I'm pretty sure my ACC and GYRO are both reading in phase, and my Kalman output all looks fine... just a little late.
Title: Re: Balancing robot for dummies
Post by: GuzZzt on Jan 08, 2011, 02:44 pm
I tried to google a bit about alternatives to PID and found some fuzzy logic and neural network papers. It looks complicated and I have no experience in this field so I don't know where to start. But using a self adaptive algorithm would be nice. Any way, if you are interested here is the links:

http://eprints.usq.edu.au/6168/1/Miller_2008.pdf

And some that requires IEEE login, I could only access the first one with my login but they all look interesting.
http://ieeexplore.ieee.org/xpl/freeabs_all.jsp?arnumber=4667312
http://ieeexplore.ieee.org/xpl/freeabs_all.jsp?arnumber=4777218
http://ieeexplore.ieee.org/xpl/freeabs_all.jsp?arnumber=5358152
Title: Re: Balancing robot for dummies
Post by: kas on Jan 08, 2011, 09:23 pm
Hi bryanpearson
Welcome to this thread,
no doubt Patrik will soon jump in to answer your question


@Patrik
Quote
Quote
What do you think about that? And in your opinion what baud rate should we use if possible?
I will experiment and be back by the end of the week

I ran this (really  ;)) striped down version of KasBot

Code: [Select]
int   STD_LOOP_TIME  = 9;            
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

void setup() {
 Serial.begin(XXXXX);
}

void loop() {
// ********************* print Debug info ********************************
 Serial.println("The quick brown fox jumps over the lazy dog");
 Serial.println("The lazy dog doesn't care and keep sleeping");
 Serial.print("Loop:");        Serial.print(lastLoopUsefulTime);
 Serial.print(",");            Serial.println(lastLoopTime);
 Serial.println();

// *********************** loop timing control **************************
 lastLoopUsefulTime = millis()-loopStartTime;
 if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
 lastLoopTime = millis() - loopStartTime;
 loopStartTime = millis();
}


The results (lastLoopUsefulTime) are, as expected, inversely proportional to the Serial data speed

9600             110 ms
19200             54 ms
38400             27 ms
57600             18 ms
115200             9 ms

Interestingly enough, 28800 bps doesn't work on my system
I used a Diecimila board powered by an ATmega168,
So try using 115200 bps, but YMMV.

Is it related with the lag observed by bryanpearson ??


@ GuzZzt
Quote
... It looks complicated...

It does  :o :o
Title: Re: Balancing robot for dummies
Post by: X-firm on Jan 08, 2011, 10:41 pm
@bryanpearson

There are some problems with the control showing the error value and I haven't found it yet..

You can do amazing things with Processing but when you should make a good GUI you half to make all controls from the ground and that takes more time than making it to what you really want. A balancing bot GUI...

I have moved on and start porting it to .net c# to be able to put more focus on the application than the individual controls.

@kas
I have also expremented with the communication between the bot and the computer and made a test program that seams to work. I will try it in my bot when the new one is finished. The last parts should com in the next week..
Code: [Select]

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

#define   LINE_END              10                     // \n
#define   SPLIT                 58                     // :

int   STD_LOOP_TIME  =          9;            

int sensorValue[3]  = { 10, 11, 12};
int sensorZero[3]  = { 0, 0, 0 };
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
int actAngle;                                                  // angles in QUIDS (360° = 2PI = 1204 QUIDS   <<<
int ACC_angle;
int GYRO_rate;
int setPoint = 0;
int drive = 0;
int error = 0;
int updateRate = 0;            //Nr of loop to skip sending and receving info from PC
long count = 0;
long last_count = 0;

float K = 1.4;
int   Kp = 3;                      
int   Ki = 1;                  
int   Kd = 6;
int   Kp_Wheel = 1;
int   Kd_Wheel = 1;
int   last_error = 0;
int   integrated_error = 0;
int   pTerm = 0, iTerm = 0, dTerm = 0;
int   pTerm_Wheel = 0, dTerm_Wheel = 0;

int error_message = 0;

float motorOffsetL = 1;        //The offset for left motor
float motorOffsetR = 1;        //The offset for right motor

byte buffer[55];

void setup() {                                  
 Serial.begin(115200);
 //Serial.begin(19200);
 buffer[0] = char('#');
 sendErrorToBuffer(1);
}

void loop() {
 
// ********************* print Debug info *************************************
 //serialOut_raw();
 //serialOut_timing();
 serialIn_GUI();       //Processing information from a pc
 serialOut_GUI();      //Sending information to pc for debug
 
// *********************** loop timing control **************************
 lastLoopUsefulTime = millis()-loopStartTime;
 if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
 if(lastLoopUsefulTime>STD_LOOP_TIME) sendErrorToBuffer(1); //Sends error to GUI
 lastLoopTime = millis() - loopStartTime;
 loopStartTime = millis();
}

int serial_type=0;
uint8_t loop_count=0;

void serialOut_GUI() {  
 if(error_message == 0)
 {
  //send PID values
  addIntToBuffer(1, pTerm);
  addIntToBuffer(3, iTerm);
  addIntToBuffer(5, dTerm);
  addIntToBuffer(7, error);
  addIntToBuffer(9, pTerm_Wheel);
  addIntToBuffer(11, dTerm_Wheel);
 
  //send PID parameter values
  addFloatToBuffer(13, K);
  addIntToBuffer(17, Kp);
  addIntToBuffer(19, Ki);
  addIntToBuffer(21, Kd);
  addIntToBuffer(23, Kp_Wheel);  
  addIntToBuffer(25, Kd_Wheel);
 
  //send Sensor values
  //send Angel values
  addIntToBuffer(27, ACC_angle);
  addIntToBuffer(29, actAngle);
  addIntToBuffer(31, sensorValue[ACC_X]);
  addIntToBuffer(33, sensorValue[ACC_Z]);
  addIntToBuffer(35, sensorValue[GYR_Y]);
 
  //send Torque values
  addIntToBuffer(37, drive);
  addFloatToBuffer(39, motorOffsetL);
  addFloatToBuffer(43, motorOffsetR);
 
  //send Settings values
  addIntToBuffer(47, setPoint);
  addIntToBuffer(49, STD_LOOP_TIME);
  addIntToBuffer(51, lastLoopUsefulTime);
  addIntToBuffer(53, lastLoopTime);
 
  Serial.write(buffer,55);
 }else{
   sendErrorToBuffer(error_message);
 }
}

void serialIn_GUI(){
 byte id;
 
 if(Serial.available() > 0){              
   byte param = Serial.read();

   delay(10);
   byte inByte[Serial.available()];
   if(Serial.read() == SPLIT){

     switch (param) {
       case 1:
         if(Serial.available() >= 2){
           Kp = readIntFromBuffer();
         }
         break;
       case 2:
         if(Serial.available() >= 2){
           Ki = readIntFromBuffer();
         }
         break;
       case 3:
         if(Serial.available() >= 2){
           Kd = readIntFromBuffer();
         }
         break;
       case 4:
         if(Serial.available() >= 4){
           K = readFloatFromBuffer();
         }
         break;
       case 5:
         if(Serial.available() >= 2){
           setPoint = readIntFromBuffer();
         }
         break;
       case 6:
         if(Serial.available() >= 4){
           motorOffsetL = readFloatFromBuffer();
         }
         break;
       case 7:
         if(Serial.available() >= 4){
           motorOffsetR = readFloatFromBuffer();
         }
         break;
       case 8:
         if(Serial.available() >= 2){
           Kp_Wheel = readIntFromBuffer();
         }
         break;
       case 9:
         if(Serial.available() >= 2){
           serial_type = readIntFromBuffer();
         }
         break;
       case 10:
         if(Serial.available() >= 2){
           Kd_Wheel = readIntFromBuffer();
         }
         break;
        case 11:
         if(Serial.available() >= 2){
           error_message = readIntFromBuffer();
         }
         break;
     } //End switch
   } //if(Serial.read() == SPLIT)
 } //if(Serial.available() > 0)
}

union u_tag {
 byte b[4];
 float fval;
} u;  

void addIntToBuffer(int index, int value)
{
 buffer[index] = lowByte(value);
 buffer[index + 1] = highByte(value);
}

void addFloatToBuffer(int index, float value)
{
 u.fval = value;
 
 buffer[index] = u.b[0];
 buffer[index + 1] = u.b[1];
 buffer[index + 2] = u.b[2];
 buffer[index + 3] = u.b[3];
}

void sendErrorToBuffer(int error){
 byte buffer_error[3];
 
 error_message = error;
 
 buffer_error[0] = char('!');
 buffer_error[1] = lowByte(error);
 buffer_error[2] = highByte(error);
 
 Serial.write(buffer_error, 3);  
}

int readIntFromBuffer(){
 uint8_t lsb = Serial.read();
 uint8_t msb = Serial.read();
 Serial.flush();
 return (lsb + (msb <<8));
}

float readFloatFromBuffer(){
 u.b[0] = Serial.read();
 u.b[1] = Serial.read();
 u.b[2] = Serial.read();
 u.b[3] = Serial.read();
 Serial.flush();
 return u.fval;
}



I have added a error handler so we can send errors to the GUI that you half to acknowledged by sending a 0 value to the error_message to get it to stop sending the error to the GUI..

What do you think about it so far? The code is real dirty but I think you understand it.. The loop time is around 4-6ms when running it...
Title: Re: Balancing robot for dummies
Post by: kas on Jan 09, 2011, 09:55 am
Quote
What do you think about it so far?

Possible options for speeding the transmital process:
- reduce number of parameters to be tansmitted
- transmit parameter only when value have changed
- if you have 50 data to transmit, split the information and try sending 10 values per cycle.
   all info will be passed within 5 cycles = 500ms, which is acceptable
- Combine these options
Title: Re: Balancing robot for dummies
Post by: kas on Jan 10, 2011, 11:05 am
Quote
hello.
I'm looking for the kasBot v2.0 code, involving quadrature encoders. Couldn't find it on the "official" site.
Can you help me?

Andreas
Norway

Hi Andreas,
Sorry for that, I will send it today to Patrik for posting in his blog
http://www.x-firm.com/?page_id=145
Title: Re: Balancing robot for dummies
Post by: BrianBot on Jan 10, 2011, 10:25 pm
I've decided to tackle an Arduino balancing robot for my final design project for my two year college program. I may make my own thread documenting my progress (similar to yours), but maybe not (your walk through is impressive).


Brian!
Title: Re: Balancing robot for dummies
Post by: BrianBot on Jan 10, 2011, 10:25 pm
I was thinking about using:

sensors: http://www.robotshop.ca/sfe-ardupilot-sensor-board-six-degrees-of-freedom-imu-main-1.html

motors & wheels: http://www.robotshop.ca/tamiya-72101-gearmotor-foam-tire-set-1.html

motor controller shield: http://www.robotshop.ca/adafruit-motor-shield-kit-arduino-4.html

Arduino controller: http://www.robotshop.ca/arduino-uno-microcontroller.html

I will settle on a battery pack and voltage regulator once I final my component choices.

The MCU is 32-bit with a 16MHz crystal, the motors' RPM is 242  and the wheels are 2.5" diameter. Are these fast, large, and good enough to balance properly? My instructor seems to think there will be problems with different torques of each motor. How much validity is in that? Any other issues I'm overlooking before I start?

It does feel like I'm not entirely sure what I'm getting myself into, but I have a few months to figure it all out. This forum will make my life much easier; I salute you  [smiley=beer.gif]


Brian!
Title: Re: Balancing robot for dummies
Post by: kas on Jan 11, 2011, 11:01 am
Hi BrianBot,
Welcome and good luck for your project  :)

Looking at your shopping list, I have the following comments:
motor controller shield: (http://www.robotshop.ca/adafruit-motor-shield-kit-arduino-4.html)
This device will definitly not cope with the amperes needed to feed your motors  :-?
Look here (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0) (initial post)

motors & wheels: (http://www.robotshop.ca/tamiya-72101-gearmotor-foam-tire-set-1.html)
The wheels are wide and soft, which is good for balancing
Wheel diameter could be higher
You need encoders (at least one). How will you afix them ??

Quote
My instructor seems to think there will be problems with different torques of each motor.
How much validity is in that?
This is addressed with the second encoder

Feel free to document you project here
This thread is now very visible from Google, this is convenient for discussion with non Arduino jerks
Title: Re: Balancing robot for dummies
Post by: BrianBot on Jan 11, 2011, 08:17 pm
I was denied permission to use shields, anyway, as it was declared "too easy". I suppose that's convenient.

I got my hands on a free L298N H-Bridge: http://www.st.com/stonline/books/pdf/docs/1773.pdf
Unfortunately it only can handle spikes of 3 A (repetitive 2.5 A). Did you ever get an idea of what currents are used? I plan on making the bot relatively small...

Also, have you ever run into speed problems from the microcontroller? Is 16 MHz fast enough?

My understanding was that the sensors act as the feedback to tell the motors direction and speed, so a motor encoder is not necessary. I see you made the same statement in your initial post.
Title: Re: Balancing robot for dummies
Post by: Big Oil on Jan 12, 2011, 12:16 am
Check out the "murata girl" a unicycle robot that balances forward/back and left/right.  It is at the 1:35 mark on the youtube video.
http://spectrum.ieee.org/tech-talk/consumer-electronics/portable-devices/ces-murata-boy-and-girl-strike-again
Title: Kalman settling time?
Post by: osmaneralp on Jan 13, 2011, 01:27 am
I'm trying to use the Kalman code described in this thread. When the angle of my bot changes rapidly, the filter can take 10 cycles to settle. In other words, the computed angle lags the accelerometer angle by as much as 10 cycles. This is making my bot slow to respond. Is this lag normal? Is there anything I can do to speed up the response?

Thanks!
Osman
Title: Re: Balancing robot for dummies
Post by: X-firm on Jan 13, 2011, 03:41 pm
@osmaneralp

I had something simulare but i found that I had the wrong value when setting the scaling of the gyro rate. Check in the data sheet for you sensor what the sensitivity is for mV/(deg/sec)

Code: [Select]

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


Also it's important that you have a good value on the ACC_Z and it should be equal when having it standing as up side down...
Quote
For ACC_Z, gravity correction = (0° value - 180° value)/2

Quote

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[ch8243]
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)


Hope you find the problem.. What sensor are you using?
Title: Re: Balancing robot for dummies
Post by: X-firm on Jan 13, 2011, 04:41 pm
I have now gotten my BlueSmirf and the other parts I was missing to be able to complete my bot.

I tried the wireless communication at 115200 bps and it worked right out of the box. Very happy with it and maybe now I can finalize the bot and the Balancing Bot GUI 2.0..

I have gotten the speed of the communication between the bot and GUI to a acceptable level. I have split ed the data in five groups and each group is sent each fifth loop..

I will post some images and a video on the interface and robot in the end of the week hopefully..  :-/
Title: Re: Balancing robot for dummies
Post by: osmaneralp on Jan 13, 2011, 10:43 pm
@Patrik

Thanks for the suggestion. I'll look more closely at the gyro. I'm using an LPY510AL connected to a MCP3302 13-bit a2d converter. I went with an external a2d to try to get more accuracy. Whether or not I need the extra resolution is not yet clear. I can explain how to connect the a2d to get all 13 bits of resolution if anyone's interested.

I'm using the 4X out instead of the 1X out from the gyro. Anyone know if this is a good idea? The 4X out gives 10mv/deg/sec. The a2d has 8096 units in a 2.48v range. That's about 0.3mv/unit. So each a2d unit represents 0.03 deg/s or 0.085 quid/s. This is quite different from the 4.58 quid/s used in this thread. Does this mean I will have to change the kalman matrix parameters?  :-/

The accelerometer is built into the Chumby that also contains the ARM processor which is the brains of my bot. I'm getting good symetrical readings from the acc, so I don't think that's my problem.

Here's a picture of my bot:
(http://lh6.ggpht.com/_Ov1LEtD4Mno/TS93_DcSnsI/AAAAAAAAABE/UP3bDFiC9Bg/s512/osman_bal_bot1.jpg)

--Osman
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Jan 14, 2011, 12:57 am
So cute! And it's blue! I love it!
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Jan 14, 2011, 01:21 am
Guys, I have a question, since I won $40 loyalty from SF, I want to get a new IMU board. Question is: should I get the recommended 5DOF IMU (http://www.sparkfun.com/products/9268) ($44.95) OR should I get the new 6DOF IMU (http://www.sparkfun.com/products/10121) that uses I2C ($64.95)?
The advantage of the last one being freeing the analog pins so I can use pots to fine tune the PID and perhaps a better accuracy. But is I2C fast enough in this case? Also the acc-meter has 2 interrupt output pins that you can program to trigger when falling too fast or when new measurement is ready, etc.
Title: Re: Balancing robot for dummies
Post by: X-firm on Jan 14, 2011, 08:44 am
@osmaneralp

I'm using a similar sensor I think and it's running in 4X mode. I got the following gyro rate as below but then I use a Arduino analog port directly.

Quote

I'm using a nother GYR called LPR510AL and that one has a sensitivity of 10 mV/(°/s) at ±100°/s. ( and it's running in 4X mode)

It looks like I have made a miss calculation where it should have been:  

// ARef=3.3V, Gyro sensitivity=10mV/(deg/sec)
// in quid/sec:(1024/360)/1024 * 3.3/0.010)
return int((sensorValue[GYR_Y] * 0,9166679)*-1);

Measurement range:       ±100°/s and ±400°/s
Sensitivity:       10 mV/(°/s) and 2.5 mV/(°/s)



@Ro-Bot-X

I know to little about the SPI buss but I love the idea to getting the analog ports free. If you buy it and it works good I will defiantly go for one later on to..


Title: Re: Balancing robot for dummies
Post by: X-firm on Jan 14, 2011, 11:20 am
@kas
@osmaneralp
@Ro-Bot-X
@GuzZzt

I would like to have a page on the projects homepage with some example robots that has used this project to be able to build a balancing robot or something else...

If you feel that this project had helped you with your bot could you send me a link to a homepage or a build blog there you describe your bots and what components you have used. With some images and videos etc.

Otherwise send some text and the images and links to videos and I can make a page on my web server for the bot..
Title: Re: Balancing robot for dummies
Post by: kas on Jan 14, 2011, 12:33 pm
Quote
I got my hands on a free L298N H-Bridge: http://www.st.com/stonline/books/pdf/docs/1773.pdf
Unfortunately it only can handle spikes of 3 A (repetitive 2.5 A). Did you ever get an idea of what currents are used? I plan on making the bot relatively small...
I would take the motor stall current as the H-bridge amp requirement

Quote
Also, have you ever run into speed problems from the microcontroller? Is 16 MHz fast enough?
16 MHz is fast enough

Quote
My understanding was that the sensors act as the feedback to tell the motors direction and speed, so a motor encoder is not necessary. I see you made the same statement in your initial post.
You will get "basic" balance w/o encoders, but the bot will drift forward/backward




Quote
I have now gotten my BlueSmirf and the other parts I was missing to be able to complete my bot.
...
I will post some images and a video on the interface and robot in the end of the week hopefully..
Congratulation Patrik
I will also post my XBee version this week end ;) ;)



Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Jan 14, 2011, 02:03 pm
Ok, I ordered the 6DOF IMU. It has I2C interface, not SPI. Standard speed is 100k/s, faster speed is 400k/s. To poll the sensor we need the Wire library, I have experience working with I2C sensors and I have 4 Arduinos on a robot linked through I2C interface. So, I hope it will work fine. The only thing I have a problem with is the 3.3V interface. I'll see what I can do. I think I need to use 4.7k pull up resistors tied to 3.3V instead of 5V. Also, the sensor needs 3.3V that I will get from the Arduino board directly (I still need to check the current requirements).

I will create a blog page with my attempt, although it did not work because of the design I tried just to see if it works. New version coming as soon as I will get the new IMU.
Title: Re: Balancing robot for dummies
Post by: kas on Jan 14, 2011, 04:29 pm
08:16 14/01/2011
@Osmaneram
Nice bot  :)
I played with a MPC3208 ADC which is the 8 input version
As far as I remember, those are 12 bit ADC's with 4096 units (not 8096)  :o
Please clarify

Your IMU board may have a different noise pattern that may require specific Kalman filter tuning.
The ADC should not inpact Kalman tuning (it didn't on my setup)



@Ro-Bot-X
I am also tempted to go digital for several reasons
- analogic is noisy by nature
- additional features
- this is the future...

I2C frequency is in the 100KHz range, don't be affraid
Please try it and let us know the outcome, we all need guinea pigs  ;) ;)


Title: Re: Balancing robot for dummies
Post by: osmaneralp on Jan 14, 2011, 06:11 pm
Quote
I played with a MPC3208 ADC which is the 8 input version
As far as I remember, those are 12 bit ADC's with 4096 units (not 8096)  
Please clarify


@kas Good catch. I'm actually using a 3302, which is a four channel 13-bit differential input a2d. The extra bit compared to the 32xx parts is a sign bit. To take advantage of the sign bit, the a2d input voltage must swing above and below the a2d vref. If the vref of your arduino is tied to 3.3v, you the input voltage from the gyro will never go above vref, and so you only get 12 bits.

To get the full 13 bits, take the vref from the gyro, which on my gyro is 1.241v, and connect it to the vref on the a2d and channel 1 on the a2d. Take the vout from the gyro and connect it to channel 0 on the a2d. Configure the a2d to run in differential mode. This means that the a2d will compare channel 0 to channel 1. If the voltage on channel 0 is greater than the voltage on channel 1, the output of the a2d is positive and the sign bit is 0, otherwise the sign bit is 1 which indicates a negative value. In addition to the sign bit, there are 12 more bits of resolution. This means that you get 12 bits from 0 to vref, and another 12 bits from vref to 2 x vref. That's 13 bits effective resolution.

Whether or not all 13 bits will be useful is not yet clear!  :-/

--Osman
Title: Re: Balancing robot for dummies
Post by: prof_jazz on Jan 14, 2011, 07:33 pm
Hi Kas,

compliments for your great job and info.

I have terminated a self balancing robot very similar to your (same IMU), except for motors.

I used 6V HITEC HS-325HB hacked to run continuosly, but this motors are not enough for he job they have to do.

I tried to make the robot less high but it became too instable..

anyway I order the same Your Pololu equipment...waiting for shipment.

Two questions:

(1) in the setup, with the function calibrateSensors(); you have to put the robot (each time you switch it on) exactly in the static equilibrium position in order to evaluate zero values (exept for z).

is this a little bit boring and could produce errors (if it is not perfectly perpendicular)?

If during the calibrateSensors() i'll give a tilt, the Robot will work to keep the tilt!

maybe there is something that I cannot understand?

(2) what do You use to recharge the 12V batteries?


thanks,

bye,

Prof
Title: Re: Balancing robot for dummies
Post by: kas on Jan 15, 2011, 11:47 am
Hi prof_jazz, welcome  :)
please let us have a photo of your new pet

Quote
I used 6V HITEC HS-325HB hacked to run continuosly, but this motors are not enough for he job they have to do
I also waste one year following this track  ;)

Quote
(1) in the setup, with the function calibrateSensors(); you have to put the robot (each time you switch it on) exactly in the static equilibrium position in order to evaluate zero values (exept for z).
is this a little bit boring and could produce errors (if it is not perfectly perpendicular)?
If during the calibrateSensors() i'll give a tilt, the Robot will work to keep the tilt!
No No...
If you tilt the bot forward before calibrateSensors(), it will move forward and find a vertical equilibrium
This will happen only with encoder(s) implementation, look here (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/124)

Quote
(2) what do You use to recharge the 12V batteries?
I ordered the battery charger shown here (http://cgi.ebay.com/Universal-Smart-Charger-4-RC-2-4V-14-4V-NiMH-RC-Battery-/220518377603?pt=LH_DefaultDomain_0&hash=item3357eb6883)



Title: Re: Balancing robot for dummies
Post by: kas on Jan 15, 2011, 12:12 pm
      [size=14] ** Balancing robot for dummies **[/size]


[size=14]Part seven: Xbee wireless control, KasBot V 3.0[/size]

The objective is to obtain basic motion control (Forward/Backward, Stop Right/Left)
Xbee modules act as a serial line replacement, a USB cable without the cable :)
See post #82 (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/75) for more info


[size=14]1) controller side:[/size]

Shopping list at post #77 (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/75)
Transmitter with Nunchuck joystick controller is shown on post #107 (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/105)
The additional micro joystick and the 3 LED's will be used for advanced features wich are beyong the scope of this basic KasControl V1.0

Nunchuk_kas.h does take care of the I2C communication with the joystick
Code: [Select]
// Nunchuck_kas.h        joystick functions ----------------------------------------------------------------

#include "Wprogram.h"
#include <Wire.h>

static uint8_t nunchuck_buf[6];                             // array to store nunchuck data,

void nunchuck_init()  {
 Wire.begin();                                                  // join i2c bus as master
 Wire.beginTransmission(0x52);                                  // transmit to device 0x52
 Wire.send(0x40);                                        // sends memory address
 Wire.send(0x00);                                        // sends a zero.  
 Wire.endTransmission();                                  // stop transmitting
}

void nunchuck_send_request()  {                             // Send a request for data to nunchuck
 Wire.beginTransmission(0x52);                                  // transmit to device 0x52
 Wire.send(0x00);                                        // sends one byte
 Wire.endTransmission();                                  // stop transmitting
}

char nunchuk_decode_byte (char x)  {                         // Encode data to format that most wiimote drivers
 x = (x ^ 0x17) + 0x17;                                     // except only needed if you use one of the regular
 return x;                                                  // wiimote drivers
}

int nunchuck_get_data()  {                                   // Receive data back from the nunchuck
 int cnt=0;
 Wire.requestFrom (0x52, 6);                                  // request data from nunchuck
 while (Wire.available ()) {                               // receive byte as an integer
   nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.receive());
   cnt++;
 }
 nunchuck_send_request();                                  // send request for next data payload
 if (cnt >= 5)     return 1;                               // If we recieved the 6 bytes, then go print them
 return 0;                                                 // failure
}                                                           // returns 1 on success read, returns 0 on failure

int nunchuck_joyx()  {                                     // returns value of x-axis joystick
 return nunchuck_buf[0];
}

int nunchuck_joyy()  {                                     // returns value of y-axis joystick
 return nunchuck_buf[1];
}

This is a strip down version of the code that can be found in the Arduino Playground section.
We only use the joystick data, (we have enough accelerometers in this project  ;))
More info on wiring and code here (http://www.windmeadow.com/node/42)

The main code uses a communication protocol based on 3 Bytes:
  controlByte = 'd'      (just says "hey, I am coming with new joystick data !!")
  dataByte_1 = value of y-axis joystick
  dataByte_2 = value of x-axis joystick

To reduce traffic, data is only sent when joystick is moved
Code: [Select]
// ** Kas Control V10     Balancing bot RC with nunchuk **  

// Yellow: Clock SCL (AI5)  Red: 3.3V  Green: Data SDA(AI4)  Write: Ground
// serial data format:   <controlByte> <dataByte_1> <dataByte_2> <CR>
// V1.0 basic control

#include <Wire.h>
#include "Nunchuck_kas.h"

int refreshTime = 100;                                    // time in millisecs between Nunchuck poolings
long lastPulse = 0;
char controlByte = 0;
char dataByte_1 = 0;                                      // joystick data
char dataByte_2 = 0;

void setup()  {
 Serial.begin(19200);
 nunchuck_init();                                         // send the initilization handshake
 delay(100);
 nunchuck_joyx();
 nunchuck_joyy();
}

void loop()  {
 if (millis() - lastPulse >= refreshTime) {                // x ms have passed
   checkNunchuck();
   lastPulse = millis();                                   // save the time of last pulse
 }
}

void checkNunchuck()  {
 static int Byte1_ant, Byte2_ant;
 nunchuck_get_data();
 controlByte = 'd';
 dataByte_1 = map(nunchuck_joyy(), 27, 218, 0, 40);        // nunchuck joystick North/South                    
 dataByte_2 = map(nunchuck_joyx(), 27, 218, 0, 40);        // nunchuck joystick East/West
 dataByte_1 = constrain(dataByte_1, 0, 40);                                
 dataByte_2 = constrain(dataByte_2, 0, 40);
 if((dataByte_1!=Byte1_ant)||(dataByte_2!=Byte2_ant))  {
   updateBot();
   Byte1_ant = dataByte_1;
   Byte2_ant = dataByte_2;
 }
}

void updateBot()    {                                       // send data stream
   Serial.print(controlByte);        
   Serial.print(dataByte_1);    
   Serial.print(dataByte_2);
   Serial.print("\n");
}


...........

Title: Re: Balancing robot for dummies
Post by: kas on Jan 16, 2011, 07:33 am
..........   continued


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


[size=14]Part seven: Xbee wireless control, KasBot V 3.0[/size]



[size=14]2) Robot side:[/size]

On the bot side, the Xbee module is set as per post #107 (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/105)

The datastream is decoded within getMotion()
Code: [Select]
void getMotion() {
 if(!Serial.available())      return;
 delay(5);                  
 int controlByte = Serial.read();              // get control byte
 if(!Serial.available())      return;
 int dataByte_1 = Serial.read();               // get data byte 1
 if(!Serial.available())      return;
 int dataByte_2 = Serial.read();               // get data byte 2
 Serial.flush();                                

 if(controlByte == 'd')   {                    // Nunchuk joystick data
   setPoint = -(dataByte_1 - 20)/2;          // get forward/backward motion byte
   if(abs(setPoint) > 2)  {
     count = 0;
     last_count = 0;
   }
   turn = (dataByte_2 - 20);                   // get right/left motion byte
 }  else    bip(bipPin, 10, 1);                  // wrong header
}


setPoint is used to bias updatePid() and slithly move the bot from vertical, thus initiating the move

Code: [Select]
void loop() {
// ********************* Get Xbee data *******************************
 getMotion();

// ********************* Sensor aquisition & filtering ***************
.....

// *********************** Angle Control and motor drive *************
 drive = updatePid(setPoint, actAngle);                                // PID algorithm

 if(abs(actAngle) < 100)    {
   Drive_Motor(drive);
 }  else {  
   Drive_Motor(0);                                                    // stop motors if situation is hopeless
   setZeroIntegal();                                                  // reset PID Integral accumulaor
 }
// ********************* print Debug info ****************************
.....

// *********************** loop timing control ***********************
.....

}



turn becomes a new parameter in Drive_Motor(), to allow right and left motor to run at their own speed
Code: [Select]
int Drive_Motor(int torque)  {
 if (torque + turn >= 0)  {                                          // drive motors forward
   digitalWrite(InA_R, LOW);                        
   digitalWrite(InB_R, HIGH);
   torque_R = torque + turn;
   if (torque - turn >= 0)  {                                        // drive motors forward
     digitalWrite(InA_L, LOW);                    
     digitalWrite(InB_L, HIGH);
     torque_L = torque - turn;
   }  else  {
     digitalWrite(InA_L, HIGH);                      
     digitalWrite(InB_L, LOW);
     torque_L = abs(torque - turn);
   }
 }  else {                                                           // drive motors backward
   digitalWrite(InA_R, HIGH);                      
   digitalWrite(InB_R, LOW);
   torque_R = abs(torque + turn);
    if (torque - turn >= 0)  {                                       // drive motors forward
     digitalWrite(InA_L, LOW);                    
     digitalWrite(InB_L, HIGH);
     torque_L = torque - turn;
   }  else  {
     digitalWrite(InA_L, HIGH);                      
     digitalWrite(InB_L, LOW);
     torque_L = abs(torque - turn);
   }
}
 torque_R = map(torque_R,0,255,K_map,255);
 torque_L = map(torque_L,0,255,K_map,255);
 torque_R = constrain(torque_R, 0, 255);
 torque_L = constrain(torque_L * motor_Offset, 0, 255);
 analogWrite(PWM_R,torque_R);
 analogWrite(PWM_L,torque_L * motor_Offset);                      // motors are not built equal...
}



OK guys, this is probably my last significant contribution to this thread  :-/
I suddently realize that I have a real life and... some other projects to explore  ::) ::)

I am sure this thread will keep on, with new bright ideas coming on
What about non linear control ??
What about one wheeled bot as shown by Big Oil (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/195) ??

"...Two wheeled balancing robots are an area of research that may well provide the future locomotion for everyday robots..."

I really enjoyed all these discussions we had on this common project
Thanks again to everybody for reading and contributing   :) :) :)

Title: Re: Balancing robot for dummies
Post by: prof_jazz on Jan 16, 2011, 11:35 am
wow kas!

any video of this new child?

bye
Title: Re: Balancing robot for dummies
Post by: kas on Jan 16, 2011, 01:10 pm
I am better at photo than video  ;)
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Jan 17, 2011, 03:09 pm
@osmaneralp

I have ordered my Chumby from Costco, it should arrive in 7-10 days at my friend in Florida then he will send it to me in Canada. So I expect about 3-4 weeks until I'll actually have it in my hands. Until then, I'll work on the chassis and the rest of the project. I would really like to take a look at your bot's build page, especially the way you used the Chumby as a robot controller. I have found some info on the net, but a more in depth info would be great.

Thanks!
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Jan 17, 2011, 06:05 pm
@Patrick

here is a link to my attempt: http://seriousrobotics.wordpress.com/2011/01/17/balancing-octobot/
Title: Re: Balancing robot for dummies
Post by: osmaneralp on Jan 18, 2011, 04:15 am
@Ro-Bot-X

One of the guys in the robotics club I belong to (The Silicon Valley HomeBrew Robotics Club) wrote an article on using the Chumby as a robot controller. You can find it on his web site: http://www.demandperipherals.com/docs.html#howto. He said you can contact him if you have any questions. I also have a fair bit experience using the Chumby as the brains of my robots. Feel free to PM me.

I don't want to hijack this thread with posts about the Chumby, so let's start a new topic or exchange PMs.

--Osman
Title: Re: Balancing robot for dummies
Post by: prof_jazz on Jan 21, 2011, 06:12 pm
Hi,

the 1 x Dual VNH2SP30 Motor Driver Carrier MD03A  (49,95$) on Pololu is out of stock till March...

Could You suggest a valid backup solution ?

The "Pololu Qik 2s12v10 Dual Serial Motor
Controller" (74,95$) http://www.pololu.com/catalog/product/1112 uses the same VNH2SP30 chip.

I have to drive the same Kas motors (Pololu Metal Gearmotor 37Dx52L mm with 64 CPR Encoder), Could You give me a feedback and more information?
I'll be happy to add  25$ if the "Pololu Qik 2s12v10 Dual Serial Motor Controller" will drive the above motors  with no difference.

will be the arduino code the same?

Thanks
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Jan 21, 2011, 11:25 pm
I'm wondering how well a Brushed Motor ESC could work with the balancing bot? They take a servo signal between 1000-2000us and convert it to linear throttle curve, forward, backward and programmable break (jumper ON=break, Off=coast). Here is a link for such a device: http://www.hobbyking.com/hobbyking/store/uh_viewItem.asp?idProduct=9090 It's just $5 for 20A! Of course, we need 2 of them, but they are cheaper than a 5 A dual motor driver...
Title: Re: Balancing robot for dummies
Post by: X-firm on Jan 23, 2011, 02:02 pm
I think the problem with brush less motors is that they don't like low speeds where they will consume a lot amps..  
Title: Re: Balancing robot for dummies
Post by: Ro-Bot-X on Jan 23, 2011, 07:35 pm
Quote
I think the problem with brush less motors is that they don't like low speeds where they will consume a lot amps..


I was talking about Brushed motor ESC, not brush less...
Title: Re: Balancing robot for dummies
Post by: kas on Oct 28, 2014, 09:17 am
-