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

indexcoef int = 0;

/////////////////////////////////////////
# Define NUMREADINGS 5 / / filter spurious Tour [average of 5 values in this case]
int index = 0, / / Index of current reading
int total = 0, / / Total
int average = 0, / / The average
///////////////////////////////////////

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   = (/ / (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 startmillis = 0;

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

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 () (
if (delta> = mydt) (/ / read every dt ms -> 1000/dt hz.

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   - PP  , / * 0.0 * / / / (Kalman)
- PP  , / * 0.1 * /
- PP  , / * 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   + = Pdot  * dt / / (Kalman)
PP   + = Pdot  * dt / / (Kalman)
Frs   + = Pdot  * dt / / (Kalman)
PP   + = Pdot  * 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   / / (Kalman)
const float C_0 = PCt_1 * PP   / / (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   + C_1 * P   (Kalman) * /
const float t_1 = C_0 * PP  ;
/ * + C_1 * P   = 0 (Kalman) * /
PP   -= K_0 * t_0; / / (Kalman)
PP   -= K_0 * t_1; / / (Kalman)
PP   -= K_1 * t_0; / / (Kalman)
PP   -= 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 *;
)

indexcoef = (indexcoef + 1)% NUMREADINGSCOEF;

totalcoef int ;

for (int j = 0 j <3 j + +) (
totalcoef [j] = 0;

for (int k = 0 k <NUMREADINGSCOEF k + +)
totalcoef [j] + = readingscoef [j] [k];

)

coefP float = (float) totalcoef  * 5 / 1024;
float coef = (float) totalcoef  * 5 / 1024;
coefD float = (float) totalcoef  * 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;
)
)
``````