Diff: main.cpp

Revision:

8:2b8cd80bea3b

Parent:

7:30613e31988e

Child:

9:79c7f239b6a0

--- a/main.cpp	Fri Nov 23 16:49:19 2018 +0000
+++ b/main.cpp	Fri Nov 23 22:08:37 2018 +0000
@@ -6,23 +6,102 @@
 
 int main(void) {
         int accel[3];//accelを3つの配列で定義。
-        int bias=0;//補正値を定義。
+        int biasx=0;//x補正値を定義。
+        float biasxsum=0;
+        float biasxav=0;
+        int biasy=0;//y補正値を定義。
+        float biasysum=0;
+        float biasyav=0;
+        int biasz=0;//z補正値を定義。
+        float biaszsum=0;
+        float biaszav=0;
+        int x;//x軸方向の加速度。
+        int y;//y軸方向の加速度。
         int z;//z軸方向の加速度。
+        float X;//加速度の変換後の値。
+        float Y;//加速度の変換後の値。
         float Z;//加速度の変換後の値。
-//以下はキャリブレーションテスト用のプログラム。値を入れてテストしてください。
-//while(1){mpu.readAccelData(accel);z = accel[2]+1150;Z = z*0.000597964111328125;printf("%f\r\n",Z);}
-    while(Z < 9.7970444){
+//Z軸方向の加速度バイアス平均値計算。
+    for(int i=0;i<2000;i++){
+        mpu.readAccelData(accel);
+        if(Z<9.797044){
+        biasz++;
+        z = accel[2]+biasz;//z軸方向の加速度
+        Z = z*0.000597964111328125; 
+        printf("%f\r\n",Z);
+        }
+        else{
+        biasz--;
+        z = accel[2]+biasz;//z軸方向の加速度
+        Z = z*0.000597964111328125; 
+        printf("%f\r\n",Z);
+        }
+        }
+    for(int i=0;i<100;i++){
+        mpu.readAccelData(accel);
+        if(Z<9.797044){
+        biasz++;
+        z = accel[2]+biasz;
+        Z = z*0.000597964111328125; 
+        printf("%f\r\n",Z);
+        }
+        else{
+        biasz--;
+        z = accel[2]+biasz;
+        Z = z*0.000597964111328125; 
+        printf("%f\r\n",Z);
+        }
+        biaszsum = biaszsum + biasz;
+        }
+        biaszav = biaszsum/101;
+        printf("bias=%f\r\n",biaszav);
+        getchar();
+//Z軸方向の加速度バイアス計算終了。
+//X軸方向の加速度バイアス平均値計算。
+    for(int i=0;i<2000;i++){
+        mpu.readAccelData(accel);
+        if(X<9.797044){
+        biasx++;
+        x = accel[0]+biasx;
+        X = x*0.000597964111328125; 
+        printf("%f\r\n",X);
+        }
+        else{
+        biasx--;
+        x = accel[2]+biasx;
+        X = x*0.000597964111328125; 
+        printf("%f\r\n",X);
+        }
+        }
+    for(int i=0;i<100;i++){
+        mpu.readAccelData(accel);
+        if(X<9.797044){
+        biasx++;
+        x = accel[0]+biasx;
+        X = x*0.000597964111328125; 
+        printf("%f\r\n",X);
+        }
+        else{
+        biasx--;
+        x = accel[0]+biasx;
+        X = x*0.000597964111328125; 
+        printf("%f\r\n",X);
+        }
+        biasxsum = biasxsum + biasx;
+        }
+        biasxav = biasxsum/101;
+        printf("bias=%f\r\n",biasxav);
+        getchar();
+//X軸方向の加速度バイアス計算終了。
+    while(1){
         mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
-        //int x = accel[0];//x軸方向の加速度
-        //int y = accel[1];//y軸方向の加速度
-        z = accel[2]+bias;//z軸方向の加速度
-        //float X = x*0.000597964111328125;
-        //float Y = y*0.000597964111328125;
+        x = accel[0]+biasx;//x軸方向の加速度
+        y = accel[1];//y軸方向の加速度
+        z = accel[2]+biasz;//z軸方向の加速度
+        X = x*0.000597964111328125;
+        Y = y*0.000597964111328125;
         Z = z*0.000597964111328125; 
-        //double a = X*X+Y*Y+Z*Z-95.982071137936;
-        //printf("%lf %f %f %f\r\n",a,X,Y,Z);
-        printf("%f\r\n",Z);
-        bias++;
+        double a = X*X+Y*Y+Z*Z-95.982071137936;
+        printf("%lf %f %f %f\r\n",a,X,Y,Z);
     }
-        printf("bias=%d\r\n",bias);
-}
+    }
\ No newline at end of file