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  [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   = (/ / (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   - 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 *; ) readingscoef  [indexcoef] = analogRead (0); readingscoef  [indexcoef] = analogRead (1); readingscoef  [indexcoef] = analogRead (2); 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]; totalcoef [j] / = NUMREADINGSCOEF; ) 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; ) )