Go Down

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

granyte

keeep going this is quite interesting

kas

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

kas

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


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

Forewords: Acc sensors are... noisy :o


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


taken from the ADXL330 data sheet

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

Code: [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:



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


kas

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

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

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

Code: [Select]
// Main module   K_bot angle    angles in Quids, 10 bit ADC -----------------------------
// 5 - angle and rate calculation       display ACC_Angle and GYRO_rate

#include <math.h>

#define   GYR_Y                 0                              // Gyro Y (IMU pin #4)
#define   ACC_Z                 1                              // Acc  Z (IMU pin #7)
#define   ACC_X                 2                              // Acc  X (IMU pin #9)

int   STD_LOOP_TIME  =          9;            

int sensorValue[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  ::)

killersquirel11

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

Go Up