File content as of revision 2:f93868975538:

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

Serial pc(USBTX, USBRX);
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);

float l_theta_k;
float l_theta_h;
float l_theta_a_x;
float l_theta_a_x_rad;
float l_theta_7_x;
float l_theta_8_x;
float l_theta_a_y;
float l_theta_a_y_rad;
float l_theta_7_y;
float l_theta_8_y;
float l_theta_8_r_y;
float l_theta_5;
float l_theta_7_m;
float l_theta_8_m;
float l_theta_6;
float h_l;
float r_theta_k;
float r_theta_h;
float r_theta_a_x;
float r_theta_a_x_rad;
float r_theta_2_x;
float r_theta_1_x;
float r_theta_a_y;
float r_theta_a_y_rad;
float r_theta_2_y;
float r_theta_1_y;
float r_theta_1_r_y;
float r_theta_4;
float r_theta_2_m;
float r_theta_1_m;
float r_theta_3;
float h_r;
float angle_read;
float angle_read_rad;
float speed_setpoint;
float speed_error;
float pos_setpoint;
float pos_error;
float kp;
float kd;
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 output_angle_x;
float output_angle_y;
int output_angle_d_x;
int output_angle_d_y;
float force_mag; 
float control_action;



int main()
{
    m5.SetGoal(150);
    wait(1);
    m6.SetGoal(150);
    wait(1);
    m7.SetGoal(150);
    wait(1);
    m8.SetGoal(150);
    wait(1);
    m1.SetGoal(150);
    wait(1);
    m2.SetGoal(150);
    wait(1);
    m3.SetGoal(150);
    wait(1);
    m4.SetGoal(150);
    wait(1);
    m9.SetGoal(150);
    wait(1);
    m10.SetGoal(150);
    wait(1);
    h_l = 0;
    h_r = 0;

    angle_x = 0;
    angle_y = 0;
    speed_setpoint = 0;
    pos_setpoint = -3;

    kd = 0;
    kp = 2;

    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.595;

        int x_msb, y_msb, z_msb;
        char x_lsb, y_lsb, z_lsb;
        short ax, ay, az;
        float afx, afy, afz;

        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;

        force_mag = abs(az) + abs(ay) + abs(ax);                     // Check force magnitude to reduce effects of shocks/vibration


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

        angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a;
        output_angle_y = angle_y;
        output_angle_d_y = output_angle_y;
        
        pos_error = pos_setpoint - output_angle_x;
        speed_error = speed_setpoint - speed_x;
        
        control_action = kp*pos_error + kd*speed_error;
        if(control_action > 8){
            control_action = 8;
        }else if(control_action < -8){
            control_action = -8;
        }else{
            control_action = control_action;
        } 
        angle_read = control_action;
        angle_read_rad = angle_read*(3.14159/180);
        
        if(angle_read == 0){
            h_l = 0;
            h_r = 0;
        }else if(angle_read > 0){
            h_l = 79*tan(angle_read_rad);
            h_r = 0;
        }else if(angle_read < 0){
            angle_read_rad = abs(angle_read_rad);
            h_l = 0;
            h_r = 79*tan(angle_read_rad);
        }
        l_theta_h = ((180/3.14159)*acos(1-(h_l/204)));
        l_theta_k = 2*l_theta_h;
        l_theta_5 = 150 + l_theta_h;
        l_theta_6 = 150 + l_theta_k;
        
        r_theta_h = ((180/3.14159)*acos(1-(h_r/204)));
        r_theta_k = 2*r_theta_h;
        r_theta_4 = 150 - r_theta_h;
        r_theta_3 = 150 - r_theta_k;
        
        l_theta_a_x = l_theta_h;
        l_theta_a_x_rad = l_theta_a_x*(3.14159/180);
        l_theta_8_x = (l_theta_a_x/0.34);
        l_theta_7_x = ((180/3.14159)*(asin((11.4/8)*l_theta_a_x_rad)));

        l_theta_a_y = angle_read;
        l_theta_a_y_rad = l_theta_a_y*3.14159/180;
        l_theta_7_y = -1.43*l_theta_a_y;
        l_theta_8_r_y = -asin((20.94/8)*sin(l_theta_a_y_rad));
        l_theta_8_y= (l_theta_8_r_y*180/3.14159);
        
        r_theta_a_x = r_theta_h;
        r_theta_a_x_rad = r_theta_a_x*(3.14159/180);
        r_theta_1_x = (r_theta_a_x/0.34);
        r_theta_2_x = ((180/3.14159)*(asin((11.4/8)*r_theta_a_x_rad)));

        r_theta_a_y = angle_read;
        r_theta_a_y_rad = r_theta_a_y*3.14159/180;
        r_theta_2_y = -1.43*r_theta_a_y;
        r_theta_1_r_y = -asin((20.94/8)*sin(r_theta_a_y_rad));
        r_theta_1_y= (r_theta_1_r_y*180/3.14159);
        
        l_theta_7_m = 150 + l_theta_7_x + l_theta_7_y;
        l_theta_8_m = 150 - l_theta_8_x + l_theta_8_y;
        
        r_theta_2_m = 150 - r_theta_2_x - r_theta_2_y;
        r_theta_1_m = 150 + r_theta_1_x - r_theta_1_y;
        
        m1.SetGoal(r_theta_1_m);
        m2.SetGoal(r_theta_2_m);
        m3.SetGoal(r_theta_3);
        m4.SetGoal(r_theta_4);
        m5.SetGoal(l_theta_5);
        m6.SetGoal(l_theta_6);
        m7.SetGoal(l_theta_7_m);
        m8.SetGoal(l_theta_8_m);
        pc.printf("\n\rAngle: %05f",output_angle_y);
           
    }
}