Diff: main.cpp

Revision:

18:2a47ed430cfe

Parent:

17:9bc130ebb5ed

Child:

19:8ad7dcfef11f

--- a/main.cpp	Tue Nov 20 06:43:02 2018 +0000
+++ b/main.cpp	Fri Nov 23 09:31:07 2018 +0000
@@ -41,7 +41,7 @@
  
 int main() {
     
-     FET = 0; 
+  /*   FET = 0; 
      while(1) {
     if(flight==1) {
         wait(10);
@@ -59,7 +59,7 @@
         FET = 0; 
     break;
             }
-    }
+    }*/
     
     motor1.stop(0);
     motor2.stop(0);
@@ -158,7 +158,7 @@
         float oldAccel = 0;//ひとつ前の加速度
         float difference=0;//変位
         float timespan=0.01;//時間差
-        float accel[3];//accelを3つの配列で定義。
+        int accel[3];//accelを3つの配列で定義。
 
         while(1)
     {       
@@ -166,9 +166,18 @@
         printf("%d\r\n",distance); 
         
         mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
-        float x = accel[0]/16384-0.043;//x軸方向の加速度
-        float y = accel[0]/16384;//y軸方向の加速度
-        float z = accel[0]/16384;//z軸方向の加速度
+        int x = accel[0];//x軸方向の加速度
+        int y = accel[1];//y軸方向の加速度
+        int z = accel[2];//z軸方向の加速度
+        float a = x^2+y^2+z^2;
+        printf("%f\r\n",a);
+        if (a>0){
+        a = sqrt(a);
+        }
+        if (a<0) {
+        a = abs(a);
+        a = -sqrt(a);
+        }
         // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
         lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient);
         // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
@@ -184,7 +193,7 @@
 
         //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
         //printf("%f,",speed);//速度を表示
-        printf("%f,",difference);//変位を表示
+        printf("%f,",difference);//変位を表示  単位m
     
         printf("%d\r\n", test.read());              //フォトインタラプタ
         printf("%d\r\n", test2.read());