Diff: main.cpp

Revision:

2:1e20c0376eb6

Parent:

1:44a18ed16bae

--- a/main.cpp	Fri Nov 02 13:11:21 2018 +0000
+++ b/main.cpp	Mon Nov 05 10:58:56 2018 +0000
@@ -54,7 +54,8 @@
 DigitalIn boardbtn(USER_BUTTON);
 static float acc_xyz[3]={0,0,0};
 static float v_xyz[3]={0,0,0};
-float displacement_xyz[3]={0,0,0};
+ float displacement_body[3]={0,0,0};
+static float displacement_world[3]={0,0,0};
 void AccelToVelocity(float ax,float ay,float az,float dt){
     static float old_acc[3]={0,0,0};
     static float old_v[3]={0,0,0};
@@ -67,14 +68,26 @@
     old_v[0]=v_xyz[0]-old_v[0];
     old_v[1]=v_xyz[1]-old_v[1];
     old_v[2]=v_xyz[2]-old_v[2];
-    v_xyz[0]=old_v[0]+delta_ax*dt;
-    v_xyz[1]=old_v[1]+delta_ay*dt;
-    v_xyz[2]=old_v[2]+delta_az*dt;
-    
-    //pc.printf("%f %f %f \n",old_acc[]);
+    v_xyz[0]+=ax*dt;
+    v_xyz[1]+=ay*dt;
+    v_xyz[2]+=az*dt;
     
     
+    displacement_body[0]+=v_xyz[0]*dt;
+    displacement_body[1]+=v_xyz[1]*dt;
+    displacement_body[2]+=v_xyz[2]*dt;
+  //  pc.printf("%f\n",dt);
+    /*displacement_body[0]+=fillter_vx*dt;
+    displacement_body[1]+=fillter_vy*dt;
+    displacement_body[2]+=fillter_vz*dt;*/
+    
     }
+/*void VelocityToDisplacement(float dt){
+    displacement_body[0]+=v_xyz[0]*dt;
+    displacement_body[1]+=v_xyz[1]*dt;
+    displacement_body[2]+=v_xyz[2]*dt;
+    
+    }*/
 int main()
 {   
     pc.baud(9600);
@@ -212,8 +225,8 @@
         delt_t = t.read_ms() - count;
         if (delt_t > 100) { // update LCD once per half-second independent of read rate
 
-    //pc.printf("ax = %f", 1000*ax);
-    //pc.printf(" ay = %f", 1000*ay);
+  //  pc.printf("ax = %f", 1000*ax);
+  //  pc.printf(" ay = %f", 1000*ay);
    // pc.printf(" az = %f  mg\n\r", 1000*az);
 
   // pc.printf("gx = %f", gx);
@@ -267,15 +280,26 @@
 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);
 //    lcd.printString(buffer, 0, 5);
 
-            myled= !myled;
-            count = t.read_ms();
+            
     acc_xyz[0]=ax;
         acc_xyz[1]=ay;
         acc_xyz[2]=az;
-        
-        AccelToVelocity(acc_xyz[0],acc_xyz[1],acc_xyz[2],float(delt_t));//deltat*1000000
-        pc.printf("%f %f %f \n",v_xyz[0],v_xyz[1],v_xyz[2]);
-        //pc.printf("%d\n",count);
+        float mag_a=sqrt(pow(acc_xyz[0],2)+pow(acc_xyz[1],2)+pow(acc_xyz[2],2));
+        AccelToVelocity(mag_a*cos(yaw),mag_a*sin(yaw),mag_a*sin(roll),float(delt_t)/10);//deltat*1000000
+        static float fillter_vx=0;
+    static float fillter_vy=0;
+    static float fillter_vz=0;
+    fillter_vx=v_xyz[0]*0.5+fillter_vx*0.5;
+    fillter_vy=v_xyz[1]*0.5+fillter_vy*0.5;
+    fillter_vz=v_xyz[2]*0.5+fillter_vz*0.5;
+        //VelocityToDisplacement(float(delt_t));
+        //pc.printf("%f\n",sqrt(displacement_body[0]*displacement_body[0]));
+        pc.printf("%f %f\n",displacement_body[0],displacement_body[1]);
+       // displacement_world[0]+=sqrt(pow(displacement_body[0],2)+pow(displacement_body[1],2)+pow(displacement_body[2],2))
+       // pc.printf("%f %f \n",v_xyz[0],v_xyz[1]);
+       // pc.printf("%f %f %f\n",fillter_vx,fillter_vy,fillter_vz);
+            myled= !myled;
+            count = t.read_ms();
             if(count > 1<<21) {
                 t.start(); // start the timer over again if ~30 minutes has passed
                 count = 0;