Diff: main.cpp

Revision:

15:75f014c4c8b8

Parent:

13:b884f5960fbf

Child:

16:44c763c32b0d

--- a/main.cpp	Thu Nov 08 11:44:46 2018 +0000
+++ b/main.cpp	Sat Nov 10 02:12:38 2018 +0000
@@ -51,7 +51,7 @@
     motor1.stop(0);
     motor2.stop(0);
     
- /*  printf("GPS start\r\n");
+ /* printf("GPS start\r\n");
    FILE *fp = fopen("/local/gps.txt", "w");  // Open "gps.txt" on the local file system for writing
     fprintf(fp, "GPS Start\r\n");
     int n;
@@ -76,13 +76,13 @@
     
      
 compass.init();                  //地磁気センサー動作
-      
+  /*    
 int i;
 for(i=0;i<20;i++)                 //地磁気測定
 {
     pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記
     wait(0.5);
-}
+}*/
 float mc1,mc2;
     mc1=3.0;
     mc2=3.0;
@@ -137,6 +137,14 @@
         rightrun=0.0;
         leftrun2=0.0;
         
+        float filterCoefficient = 0.3; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。
+        float lowpassValue = 0;
+        float highpassValue = 0;
+        float speed = 0;//加速度時から算出した速度
+        float oldSpeed = 0;//ひとつ前の速度
+        float oldAccel = 0;//ひとつ前の加速度
+        float difference=0;//変位
+        float timespan=0.1;//時間差
         float accel[3];//accelを3つの配列で定義。         加速度センサー
 
         while(1)
@@ -145,11 +153,24 @@
         printf("%d\r\n",distance); 
         
         mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
-        float x = accel[0]/16384;//x軸方向の加速度
-        float y = accel[1]/16384;//y軸方向の加速度
+        float x = accel[0]/16384-0.043;//x軸方向の加速度
+        float y = accel[1]/16384-0.012;//y軸方向の加速度
         float z = accel[2]/16384;//z軸方向の加速度
         printf("accel x:%f y:%f z:%f\r\n",x,y,z);//一応表示しとくやで～～
-    
+        // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
+        lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient);
+        // ハイパスフィルター(センサの値 - ローパスフィルターの値)
+        highpassValue = x - lowpassValue;
+        
+        // 速度計算(加速度を台形積分する)
+        speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
+        oldAccel = highpassValue;
+        
+        // 変位計算(速度を台形積分する)
+        difference = ((speed + oldSpeed) * timespan) / 2 + difference;
+        oldSpeed = speed;
+        
+        printf(" speed %f difference %f\n",speed,difference);
     
         printf("%d\r\n", test.read());              //フォトインタラプタ
         printf("%d\r\n", test2.read());
@@ -160,7 +181,7 @@
         else if (test.read() == 0 and flag == 1){   //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
             flag = 0;                               //まずこれでスイッチを0にして入れる。
                                                     //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
-            rightrun += 52.5;                            //総距離runに52.5を加算する
+            rightrun += 54.95;                            //総距離runに52.5を加算する
             printf("test.read else\r\n");
         }
          if (test2.read() == 1 and flag2 == 0){
@@ -169,12 +190,12 @@
         }
         else if (test2.read() == 0 and flag2 == 1){
             flag2 = 0;
-            leftrun2 += 52.5;
+            leftrun2 += 54.95;
             printf("test2.read else\r\n");
         }
         printf("%f", rightrun);
         printf("\t%f\r\n", leftrun2);
-        if (rightrun > 525){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+        if (rightrun >= 4396){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
             break;                                  //つまりゴールについたらこのループからぬける
         }
         
@@ -268,7 +289,7 @@
 
             motor1.speed(-msj1);      //機体を反時計回りに９０度回転
             motor2.speed(msj2);
-            wait(0.77);
+            wait(1);
             printf("mortor rotation\r\n");
             
             motor1.stop(0);
@@ -288,7 +309,7 @@
 
             motor1.speed(msj1);       //機体を時計回りに９０度回転
             motor2.speed(-msj2);
-            wait(0.77);   
+            wait(1);   
             printf("mortor rotation\r\n");
             
             motor1.stop(0);