Diff: main.cpp

Revision:

0:e60a2d5cccc2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 28 11:12:37 2016 +0000
@@ -0,0 +1,76 @@
+#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;
+    }
+}
\ No newline at end of file