Diff: main.cpp

Revision:

16:44c763c32b0d

Parent:

15:75f014c4c8b8

Child:

17:9bc130ebb5ed

--- a/main.cpp	Sat Nov 10 02:12:38 2018 +0000
+++ b/main.cpp	Thu Nov 15 09:18:24 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;
@@ -70,19 +70,19 @@
 
     }    
     fprintf(fp,"GPS finish\r\n");
-    fclose(fp);                      //GPSの測定終了 */
+    fclose(fp);                     // GPSの測定終了 
     
   
     
      
 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;
@@ -145,7 +145,7 @@
         float oldAccel = 0;//ひとつ前の加速度
         float difference=0;//変位
         float timespan=0.1;//時間差
-        float accel[3];//accelを3つの配列で定義。         加速度センサー
+        float accel[3];//accelを3つの配列で定義。         加速度センサー*/
 
         while(1)
     {       
@@ -256,6 +256,8 @@
 
             motor1.speed(ms1);       //直進
             motor2.speed(ms2);
+            
+            
         int t=0;
     for(t=0;t<100;t++)
     {
@@ -279,7 +281,32 @@
         }
         printf("%f", rightrun);
         printf("\t%f\r\n", leftrun2);
-    }        
+        
+        
+        
+        mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
+        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("mortor straight\r\n");   
             
             motor1.stop(0);