File content as of revision 0:51b08ae8fc55:

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

Serial pc(USBTX, USBRX);
L3G4200D gyro(p9,p10);
BMA180 my_BMA180(p5,p6,p7,p15,p16);

float data_x[2];
float data_y[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 output_angle_x;
float output_angle_y;
int output_angle_d_x;
int output_angle_d_y;
float force_mag;
Timer t; 
float dt;

int main()
{

    angle_x = 0;
    angle_y = 0;    
    t.start();
    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;
                                                                                                                                
        dt = t.read_us();                                               // timer to determine dt
        //pc.printf("\n\rTime: %05f", dt);
        t.reset();
        
        //pc.printf("$%i %i;", output_angle_d_x, output_angle_d_y);     //Print values for serial reader app
        pc.printf("%d\n", output_angle_d_x);                      //Print values for MATLAB   
        //pc.printf("\n\rSpeed: %05f",speed);
        
    }
}