self balancing robot - first try

Hi guys We are a group of nerds ;D that is working on a self balancing robot in sunday workshops here in Milan. http://www.gioblu.com
We decide to use servo motors and Sparkfun IMU 5 main 5 degrees of freedom. We have built a proto with 3 potentiometers and two digital buttons and females of pinhead for sparkfun IMU. Three potentiometers are used to adjust the PID system similar to Nicola Lugato with 3 values (kP, kI, KD) (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1225283209/0), the two buttons are used to regulate the correct inclination (balance stand position) although we used a different method for the acquisition of value of potentiometers and some finishing touches.

The software combines and filters (using Kalman filter) the arc tangent of the two axis of accelerometer and the value obtained from the gyroscope, the result computed with the correction algorithm PID generates reaction.

Experimentally speaking, is quite appropriate to start from a basis of development with low-centered mass. With this form seems to be very light to change direction.

There is a big issue that is definitely the delay of the reaction. If the 3 k of PID are low, the delayed reaction it don’t works well and robot fall down, the only way to remain “quite stable” is to raise kP kI kD values, but it get in a kind of resonance (see video) oscillating continuosly. Does anyone have any ideas or advice?

Here it is the code:

# include <Servo.h> 
# Include <math.h> 

float myangle = 0; 
float pid = 0; 

float P = 0.0, / / P statement 
float I = 0.0, / / The Declaration 
float D = 0.0, / / declaration 

Long calibration = 90; 
calibrationUP int = 6; 
calibrationDOWN int = 5; 

# Define NUMREADINGSCOEF 10 
readingscoef int [3] [NUMREADINGSCOEF]; 
indexcoef int = 0; 


///////////////////////////////////////// 
# Define NUMREADINGS 5 / / filter spurious Tour [average of 5 values in this case] 
int readings [NUMREADINGS] / / Read the gyroscope 
int index = 0, / / Index of current reading 
int total = 0, / / Total 
int average = 0, / / The average 
inputPin int = 0, / / Analog input Gyro 
/////////////////////////////////////// 

float dt = .1 / / .06 / / (1024.0 * 256.0) / 16000000.0; (Kalman) / / What Does This Means? Hand tuned 
 int mydt = 1, / / in ms. 
 static float PP [2] [2] = (/ / (Kalman) 
 ( 
1, 0) 
 / / (Kalman) 
 ( 
0, 1) 
 / / (Kalman) 
) / / (Kalman) 


/ / The two states, and bias angle of the gyroscope. As a byproduct 
/ / Computation of the angle, angular velocity also get clean 
/ / The error [bias] 

float angle = 0.0, / / (Kalman) 
q_bias float = 0.0, / / (Kalman) 
float rate = 0.0, / / (Kalman) 
q_m float = 0.0, / / (Kalman) 

double ax_m = 0; 
double ay_m = 0; 
int cnt = 0; / / Counter 
unsigned long lastread = 0 
unsigned long startmillis = 0; 

/ / R is the value of convarianza reading. 
/ / Here We use a 1x1 matrix that produces a 'hypothesis 
/ / Distortion reading of 0.3 rad 

float R_angle = .025; 


/ / Q is a 2x2 matrix where the covariance is processed. 
/ / This structure allows us to understand how, in relation to 
/ / Covariance itself, you can trust accelerometer or gyroscope 
  
static const float Q_angle = 0001 / / (Kalman) 
static const float Q_gyro = 0001 / / (Kalman) 
ledPin int = 13; 

displayState byte = 0; 
oldAngle float = 0.0; 
Servant left; 
Servo right; 

void setup () ( 
 left.attach (10); 
 right.attach (9); 
 pinMode (ledPin, OUTPUT); 
 Serial.begin (19200); 
  
 for (int i = 0; i <NUMREADINGS; i + +) 
 readings [i] = 0, / / initialize all the readings to 0 (gyro average filter) 
 Millis startmillis = (); 
 ) 
 float sqr (float a) ( 
 return a * a; 
 ) 
 float sgn (float) ( 
 if (a> 0) 
return +1; 
 else 
if (a <0) 
return -1; 
 else 
return 0; 
) 

void loop () ( 
 int delta = Millis ()-lastread; 
 if (delta> = mydt) (/ / read every dt ms -> 1000/dt hz. 
 Millis = lastread (); 
  
 total -= readings [index], / / Subtract the last reading gyroscope 
 readings [index] = analogRead (3), / / Read gyroscope 
 total + = readings [index], / / Add the total reading 
 index = (index + 1) / / Update index 

 if (index> = NUMREADINGS) / / If we are to the end of 
index = 0, / / Top 

 average = (total / NUMREADINGS) / / compute the average input 

 dt = ((float) delta) / 1000.0; 

 q_m = ((float) average) * (1500.0/1024.0) * PI/180 / / HAC remove 1.5 mult 

 / / We remove the spurious from the gyroscope 
 const float q = q_m - q_bias / / (Kalman) 

 Pdot const float [2 * 2] = ( 
Q_angle - PP [0] [1] - PP [1] [0], / * 0.0 * / / / (Kalman) 
- PP [1] [1], / * 0.1 * / 
- PP [1] [1], / * 1.0 * / 
Q_gyro / * 1.1 * / 
 ); 

 / / Save the estimated value rate gyroscope clean 
 rate = q / / (Kalman) 

 / * 
/ / Been updated angle estimate 

 / * Update the covariance matrix * / 
 PP [0] [0] + = Pdot [0] * dt / / (Kalman) 
 PP [0] [1] + = Pdot [1] * dt / / (Kalman) 
 Frs [1] [0] + = Pdot [2] * dt / / (Kalman) 
 PP [1] [1] + = Pdot [3] * dt / / (Kalman) 


 / / Enter the PIN of the two axis accelerometer 
 ax_m analogRead = (4) - 338 / / 338 is a value used to adjust the degrees 
 ay_m analogRead = (5) - 338, / / ditto 
 const float angle_m = atan2 (ay_m, ax_m) / / (Kalman) 
 const float angle_err = angle_m - angle / / (Kalman) 
 const float C_0 = 1, / / (Kalman) 
 const float C_0 = PCt_0 * PP [0] [0] / / (Kalman) 
 const float C_0 = PCt_1 * PP [1] [0] / / (Kalman) 
 const float E = R_angle PCt_0 + C_0 * / / (Kalman) 
 const float K_0 = PCt_0 / E / / (Kalman) 
 const float K_1 = PCt_1 / E / / (Kalman) 
 const float t_0 = PCt_0; 
 / * C_0 * P [0] [0] + C_1 * P [1] [0] (Kalman) * / 
 const float t_1 = C_0 * PP [0] [1]; 
 / * + C_1 * P [1] [1] = 0 (Kalman) * / 
 PP [0] [0] -= K_0 * t_0; / / (Kalman) 
 PP [0] [1] -= K_0 * t_1; / / (Kalman) 
 PP [1] [0] -= K_1 * t_0; / / (Kalman) 
 PP [1] [1] -= K_1 * t_1; / / (Kalman) 
 angle + = K_0 * angle_err / / (Kalman) 
 q_bias + = K_1 * angle_err, / / (Kalman) 

 / / VALUE CALIBRATION 
  
 if (digitalRead (calibrationUP) == HIGH) ( 
 Calibration calibration = + 1; 
 delay (100); 
 ) 
 if (digitalRead (calibrationDOWN) == HIGH) ( 
 Calibration calibration = - 1; 
 delay (100); 
 ) 
  
 myangle = (angle * 57.2957795130823) + calibration; 
 if ((Millis ()-startmillis)> 6000) ( 
 rate_deg float rate = 57.2957795130823 *; 
 ) 
  
 readingscoef [0] [indexcoef] = analogRead (0); 
 readingscoef [1] [indexcoef] = analogRead (1); 
 readingscoef [2] [indexcoef] = analogRead (2); 
  
 indexcoef = (indexcoef + 1)% NUMREADINGSCOEF; 
  
 totalcoef int [3]; 
  
 for (int j = 0 j <3 j + +) ( 
 totalcoef [j] = 0; 
  
 for (int k = 0 k <NUMREADINGSCOEF k + +) 
totalcoef [j] + = readingscoef [j] [k]; 

 totalcoef [j] / = NUMREADINGSCOEF; 
 ) 
  
 coefP float = (float) totalcoef [0] * 5 / 1024; 
 float coef = (float) totalcoef [1] * 5 / 1024; 
 coefD float = (float) totalcoef [2] * 5 / 1024; 
  
 * P = myangle coefP; 
 I = (I + myangle) * coef; 
  
 if (I * myangle <0) 
 I = 0; 
  
 D = (myangle - oldAngle) * coefD; 
  
 pid = P + I + D; 
  
 if (pid> = 90) 
 pid = 90; 
  
 if (pid <= -90) 
 pid = -90; 
  
 left.write (90 - pid); 
 right.write (90 + pid); 
  
 Serial.print (P); 
 Serial.print (coefP); 
 Serial.print ("I"); 
 Serial.print (coef); 
 Serial.print key (D); 
 Serial.print (coefD); 
 Serial.print ("A"); 
 Serial.print (myangle); 
 Serial.print ("C"); 
 Serial.println (calibration); 

  
 oldAngle = myangle; 
 ) 
)