Diff: main.cpp

Revision:

1:4df856c1b9d8

Parent:

0:84dda456d02c

diff -r 84dda456d02c -r 4df856c1b9d8 main.cpp
--- a/main.cpp	Fri Jan 11 00:58:23 2013 +0000
+++ b/main.cpp	Fri Dec 18 13:15:16 2015 +0000
@@ -4,12 +4,20 @@
 DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
 MPU6050 mpu;
-
-int16_t ax, ay, az;
-int16_t gx, gy, gz;
+Timer t;
 
 int main()
 {
+    int16_t ax, ay, az;//acceleration
+    int16_t gx, gy, gz;//angular velocity
+    
+    double gx0,gy0,gz0;//angular velocity
+    double theta_x,theta_y,theta_z,theta_x0,theta_y0,theta_z0;//angle
+
+    double p, q, r;
+
+    double t_read = 0;
+    
     pc.printf("MPU6050 test\n\n");
     pc.printf("MPU6050 initialize \n");
 
@@ -22,11 +30,70 @@
     } else {
         pc.printf("MPU6050 test failed \n");
     }
-   
+
+    /***************calibration***************/
+    double sum_x,sum_y,sum_z,ave_x,ave_y,ave_z;
+
+    sum_x = 0;
+    sum_y = 0;
+    sum_z = 0;
+    
+    pc.printf("Calibration start ...\nJust a moment.\n");
+    
+    //ドリフト補正(1000回値をとって、その平均値を算出)
+    for(int i=0;i<1000;i++){
+        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+        sum_x = sum_x+gx;
+        sum_y = sum_y+gy;
+        sum_z = sum_z+gz;
+        wait(0.01);
+    }
+    
+    ave_x = sum_x/1000;
+    ave_y = sum_y/1000;
+    ave_z = sum_z/1000;
+    
+    pc.printf("Calibration result:%f;%f;%f\n", ave_x, ave_y, ave_z);
+
+    /*************main*************/
+    gx0 = 0;
+    gy0 = 0;
+    gz0 = 0;
+    theta_x0 = 0;
+    theta_y0 = 0;
+    theta_z0 = 0;
+    
     while(1) {
-        wait(1);
+        //timer start.
+        t.start();
         mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-        //writing current accelerometer and gyro position 
-        pc.printf("%d;%d;%d;%d;%d;%d\n",ax,ay,az,gx,gy,gz);
+        
+        //ドリフト補正＆スケーリング:scalling factor = 131[LSB/(°/s)]
+        p = ((double)gx-ave_x) / 131;
+        q = ((double)gy-ave_y) / 131;
+        r = ((double)gz-ave_z) / 131;
+
+        pc.printf("gx:%7.2f gy:%7.2f gz:%7.2f ",p, q, r);
+        
+        //台形積分
+        theta_x = (gx0+p)/2*t_read+theta_x0;
+        theta_y = (gy0+q)/2*t_read+theta_y0;
+        theta_z = (gz0+r)/2*t_read+theta_z0;
+        
+        pc.printf("theta_x:%7.0f theta_y:%7.0f theta_z:%7.0f\n",theta_x,theta_y,theta_z);
+
+        gx0 = p;
+        gy0 = q;
+        gz0 = r;
+        theta_x0 = theta_x;
+        theta_y0 = theta_y;
+        theta_z0 = theta_z;
+        
+        wait(0.1);
+        
+        //timer reset & stop.
+        t.stop();
+        t_read = t.read();
+        t.reset();
     }
 }