File content as of revision 2:056f36912bf0:

#include "mbed.h"
#include "BMA180.h"
#include "math.h"
#include "L3G4200D.h"
#include "AX12.h"

L3G4200D gyro(p9,p10);
BMA180 my_BMA180(p5,p6,p7,p15,p16);

AX12 m1 (p28, p27, 1);
AX12 m2 (p28, p27, 2);
AX12 m3 (p28, p27, 3);
AX12 m4 (p28, p27, 4);
AX12 m5 (p28, p27, 5);
AX12 m6 (p28, p27, 6);
AX12 m7 (p28, p27, 7);
AX12 m8 (p28, p27, 8);
AX12 m9 (p28, p27, 9);
AX12 m10 (p28, p27, 10);

Timer t_1;
Timer t_2;

float theta_1;
float theta_1_r;
float theta_2;
float theta_8;
float theta_8_r;
float theta_7;
float time_1;
float time_2;
float angle_x;
float angle_x_a;
float angle_y;
float angle_y_a;
float r1;
float r2;
float pi = 3.14159;
float speed_x;
float speed_y;
float g[3];
float theta_roll;
float theta_a_right;
float theta_a_left;
float theta_a_right_rad;
float theta_a_left_rad;
float time_sum;
float period;
int n;
float A;
float B;
float C;
float leg_control_signal;
float theta_4;
float theta_5;
int dir_0;
int dir_1;
int x_msb, y_msb, z_msb;
char x_lsb, y_lsb, z_lsb;
short ax, ay, az;
float afx, afy, afz;

int main()
{
    m1.SetGoal(150);
    wait(0.5);
    m2.SetGoal(150);
    wait(0.5);
    m3.SetGoal(150);
    wait(0.5);
    m4.SetGoal(150);
    wait(0.5);
    m5.SetGoal(150);
    wait(0.5);
    m6.SetGoal(150);
    wait(0.5);
    m7.SetGoal(150);
    wait(0.5);
    m8.SetGoal(150);
    wait(0.5);
    m9.SetGoal(150);
    wait(0.5);
    m10.SetGoal(150);
    wait(0.5);

    t_1.start();
    t_2.start();
    time_sum = 0;
    n = 0;
    angle_y = 0;
    dir_1 = 0;
    dir_0 = 0;

    while (1) {
        gyro.read(g);                                  // Read and Interpret Gyro Data
        speed_x = ((g[1])*0.07) - 1.12;
        speed_y = ((g[0])*0.07) - 0.3;

        x_lsb = my_BMA180.readReg(ACCXLSB);                              // Read X LSB register
        x_msb = my_BMA180.readReg(ACCXMSB);                              // Read X MSB register
        ax = (x_msb << 8) | x_lsb;                             // Concatinate X MSB and LSB
        ax = ax >> 2;                                          // Remove unused first 2 LSB (16 bits to 14 bits)
        afx = (float)ax*3/16384;

        y_lsb = my_BMA180.readReg(ACCYLSB);                              // Read Y LSB register
        y_msb = my_BMA180.readReg(ACCYMSB);                              // Read Y MSB register
        ay = (y_msb << 8) | y_lsb;                             // Concatinate Y MSB and LSB
        ay = ay >> 2;                                          // Remove unused first 2 LSB
        afy = (float)ay*3/16384;

        z_lsb = my_BMA180.readReg(ACCZLSB);                              // Read Z LSB register
        z_msb = my_BMA180.readReg(ACCZMSB);                              // Read Z MSB register
        az = (z_msb << 8) | z_lsb;                             // Concatinate Z MSB and LSB
        az = az >> 2;                                          // Remove unused first 2 LSB
        afz = (float)az*3/16384;

        r1 =   afx / (sqrt(pow(afy,2) + pow(afz,2)));                // Determine X component of gravity force
        r2 =   afy / (sqrt(pow(afx,2) + pow(afz,2)));                // Determine Y component of gravity force
        angle_x_a = atan(r1)*180/pi;                                 // Determine X, Y angles
        angle_y_a = atan(r2)*180/pi;

        angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a;

        angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a;
        theta_roll = angle_y + 4.5;

        dir_0 = dir_1;

        if(theta_roll >= 0) {
            dir_1 = 1;
        } else {
            dir_1 = 0;
        }

        if(dir_1 != dir_0) {
            time_2 = t_2.read();
            t_2.reset();
            time_sum = time_sum + time_2;
            n=n+1;
        }

        period = 2*(time_sum/n);
        B = 2*3.14159/period;
        C = -(period/4)*B;
        A = 5;

        if(theta_roll < -3) {
            theta_a_right = -7;
        } else {
            theta_a_right = 0;
        }
        if(theta_roll > 3) {
            theta_a_left = 7;
        } else {
            theta_a_left = 0;
        }

        time_1 = t_1.read();
        leg_control_signal = A*sin(B*time_1 + C);

        theta_4 = 150 + leg_control_signal;
        theta_5 = 150 + leg_control_signal;

        theta_a_right_rad = theta_a_right*3.14159/180;
        theta_a_left_rad = theta_a_left*3.14159/180;
        theta_2 = -1.43*theta_a_right + 150;
        theta_7 = -1.43*theta_a_left + 150;
        theta_1_r = -asin((20.94/8)*sin(theta_a_right_rad));
        theta_1 = (theta_1_r*180/3.14159)+150;
        theta_8_r = -asin((20.94/8)*sin(theta_a_left_rad));
        theta_8 = (theta_8_r*180/3.14159)+150;
        m1.SetGoal(theta_1);
        m2.SetGoal(theta_2);
        m7.SetGoal(theta_7);
        m8.SetGoal(theta_8);
        m4.SetGoal(theta_4);
        m5.SetGoal(theta_5);
    }
}