Diff: main.cpp

Revision:

3:4bd8dff84324

Parent:

2:3881894aaff5

--- a/main.cpp	Thu Aug 06 12:25:49 2015 +0000
+++ b/main.cpp	Mon Nov 05 10:55:41 2018 +0000
@@ -43,7 +43,7 @@
 Ticker toggler1;
 Ticker filter;   
 Ticker compass;        
-
+Timer tt;
 void toggle_led1();
 void toggle_led2();
 void compFilter();
@@ -52,20 +52,76 @@
 float pitchAngle = 0;
 float rollAngle = 0;
 float yawAngle = 0;
-
+float ve[3] ={0,0,0};
+float n_a[3]={0,0,0};
+static float s_b[3]={0,0,0};
+void AcceltoVelo(float aax,float aay,float aaz,float delttat)
+{ int cc=0;
+    static float old_a[3]={0,0,0};
+    float deax,deay,deaz;
+    
+    deax=aax-old_a[0];
+    deay=aay-old_a[1];
+    deaz=aaz-old_a[2];
+    ve[0]=deax*delttat;
+    ve[1]=deay*delttat;
+    ve[2]=deaz*delttat;
+old_a[0]=aax;
+    old_a[1]=aay;
+    old_a[2]=aaz;
+    //pc.printf("%f %f %f\n",deax,deay,deaz);
+}
+void VelotoDis(float vvx,float vvy,float vvz,float delttat){
+    s_b[0]+=vvx*delttat;
+    s_b[1]+=vvy*delttat;
+    s_b[2]+=vvz*delttat;
+    }
+void cali(){
+    int c=0;
+    float c_ax=0,c_ay=0,c_az=0;
+    while (c<1000){
+        c_ax+=ax;
+        c_ay+=ay;
+        c_az+=az;
+        c+=1;
+        wait(0.0001);
+        }
+    pc.printf("%f %f %f\n",c_ax/1000,c_ay/1000,c_az/1000);
+    }
 int main() 
-{
-    pc.baud(9600);                                 // baud rate: 9600
+
+{static float vvv[3]={0,0,0}; 
+tt.start();
+    pc.baud(115200);                                 // baud rate: 9600
     mpu6050.whoAmI();                              // Communication test: WHO_AM_I register reading 
     mpu6050.calibrate(accelBias,gyroBias);         // Calibrate MPU6050 and load biases into bias registers
     mpu6050.init();                                // Initialize the sensor
     hmc5883l.init();
     filter.attach(&compFilter,    0.005);              // Call the complementaryFilter func.  every 5 ms (200 Hz sampling period)
     compass.attach(&tiltCompensatedAngle, 0.015);      // Call the tiltCompensatedAngle func. every 15 ms (75 Hz sampling period)
+    //cali();
+    float cc0=0;
+    int dd=0;
     while(1) 
-    {
-        pc.printf("%.1f,%.1f,%.1f\r\n",rollAngle,pitchAngle,yawAngle);  // send data to matlab
-        wait_ms(40);
+    {  
+    AcceltoVelo(ax*10,ay*10,az*10,3);
+    vvv[0]=ve[0]*0.02+vvv[0]*0.98;
+    vvv[1]=ve[1]*0.02+vvv[1]*0.98;
+    vvv[2]=ve[2]*0.02+vvv[2]*0.98;
+    //wait_ms(4);
+        //pc.printf("%f\n",vvv[2]);
+        //pc.printf("%f %f %f\r\n",vvv[0],vvv[1],vvv[2]);
+      /*f(tt.read_us()-cc0>3000 && dd==0){
+        dd=1;
+        VelotoDis(vvv[0],vvv[1],vvv[2],6);
+        pc.printf("%f %f %f\r\n",s_b[0],s_b[1],s_b[2]);cc0=tt.read_us();
+        }else{
+            VelotoDis(vvv[0],vvv[1],vvv[2],6);
+        pc.printf("%f %f %f\r\n",s_b[0],s_b[1],s_b[2]);cc0=tt.read_us();
+        }*/
+        pc.printf("%f %f %f\r\n",ax,ay,az);
+        //pc.printf("%.1f,%.1f,%.1f\r\n",rollAngle,pitchAngle,yawAngle);  // send data to matlab
+        wait_us(3);
     }
 }