File content as of revision 0:e60a2d5cccc2:

#include "mbed.h"
#include "l3gd20.h"
#include "mma7361.h"
#include "kalman.h"
#include "binary.h"

l3gd20 a;
mma7361 b;
Kalman c;
DigitalOut myled(LED1);
Serial pc(USBTX,USBRX);
DigitalIn myb (USER_BUTTON);
Timer t;

int main() {
    int us,d1,d2;
    myled=1;
    while(1) {
        if(myb==0){
            break;
        }
    }
    
    myled=0;
    wait(1);
    while(a.conect());
    pc.printf("success\n\r");
    a.s_write_8(CTRL_REG1,b0001111);
    a.s_write_8(CTRL_REG5,b0000000);
    b.clear_mat();
    int x,z,fdata;
    int16_t data;
    b.set_1st_bit(&x,x_axis,0);
    b.set_1st_bit(&z,z_axis,1);
    float kx,gtheta,dt,atheta,xt,om,fom,iom;
    c.setAngle(0);
    atheta=0;
    fdata=0;
    fom=0;
    iom=0;
    gtheta=0;
    while(1){
        wait_ms(1);
        d1=a.read_8(OUT_Z_H);
        d2=a.read_8(OUT_Z_L);
        data=(d1<<8)|d2;
        b.low_pass_filter(&x,x_axis);
        xt=b.bit_to_g(x,x_axis);
        xt=xt*981/100;
        om=sqrt(abs(xt*1000/37));
        om=b.rad_to_deg(om);
        if(om/data<0){
            om*=-1;
        }
        iom=iom*0.9+om*0.1;
        /*b.low_pass_filter(&z,z_axis);
        atheta=b.rotate(&x,x_axis,&z,z_axis);
        atheta=b.rad_to_deg(atheta);*/
        t.stop();
        us=t.read_us();
        t.reset();
        t.start();
        dt=us*0.000001;
        gtheta+=(data+fdata)*0.00875*dt*0.5;
        atheta+=(om+fom)*dt*0.5;
        /*if(atheta/kx<0 && abs(kx)>70){
            atheta*=-1;
        }*/
        kx = c.getAngle(iom,gtheta,dt);
        pc.printf("%.3f,%.3f,%d\n\r",kx,atheta,data);
        //pc.printf("%.3f\n\r",om);
        fdata=data;
        fom=om;
        gtheta=kx;
    }
}