Diff: main.cpp

Revision:

19:8ad7dcfef11f

Parent:

18:2a47ed430cfe

Child:

20:e78bffff80ee

--- a/main.cpp	Fri Nov 23 09:31:07 2018 +0000
+++ b/main.cpp	Sun Nov 25 04:12:06 2018 +0000
@@ -64,6 +64,8 @@
     motor1.stop(0);
     motor2.stop(0);
     
+    wait(30);     //確認用
+    
  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");
@@ -83,7 +85,7 @@
 
     }    
     fprintf(fp,"GPS finish\r\n");
-    fclose(fp);                     // GPSの測定終了 
+ //   fclose(fp);                     // GPSの測定終了 
     
   
     
@@ -166,11 +168,13 @@
         printf("%d\r\n",distance); 
         
         mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
-        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);
+        int x = accel[0]-123;//x軸方向の加速度
+        int y = accel[1]+60;//y軸方向の加速度
+        int z = accel[2]+1110 ;//z軸方向の加速度
+        float X = x*0.000597964111328125;
+        float Y = y*0.000597964111328125;
+        float Z = z*0.000597964111328125; 
+        double a = X*X+Y*Y+Z*Z-95.982071137936;
         if (a>0){
         a = sqrt(a);
         }
@@ -178,10 +182,11 @@
         a = abs(a);
         a = -sqrt(a);
         }
+        //printf("%lf %f %f %f\r\n",a,X,Y,Z);
         // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
-        lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient);
+        lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient);
         // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
-        highpassValue = x - lowpassValue;
+        highpassValue = a - lowpassValue;
 
         // 速度計算(加速度を台形積分する)
         speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
@@ -192,8 +197,7 @@
         oldSpeed = speed;
 
         //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
-        //printf("%f,",speed);//速度を表示
-        printf("%f,",difference);//変位を表示  単位m
+        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示
     
         printf("%d\r\n", test.read());              //フォトインタラプタ
         printf("%d\r\n", test2.read());
@@ -222,6 +226,11 @@
             break;                                  //つまりゴールについたらこのループからぬける
         }
         
+         if(difference >= 0.3)
+         {
+             break;
+         }    
+        
         motor1.speed(0.5);   //通常走行
         motor2.speed(0.5);
                                //Do something else here
@@ -308,13 +317,25 @@
         
         
         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]-123;//x軸方向の加速度
+        int y = accel[1]+60;//y軸方向の加速度
+        int z = accel[2]+1110 ;//z軸方向の加速度
+        float X = x*0.000597964111328125;
+        float Y = y*0.000597964111328125;
+        float Z = z*0.000597964111328125; 
+        double a = X*X+Y*Y+Z*Z-95.982071137936;
+        if (a>0){
+        a = sqrt(a);
+        }
+        if (a<0) {
+        a = abs(a);
+        a = -sqrt(a);
+        }
+        //printf("%lf %f %f %f\r\n",a,X,Y,Z);
         // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
-        lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient);
+        lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient);
         // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
-        highpassValue = x - lowpassValue;
+        highpassValue = a - lowpassValue;
 
         // 速度計算(加速度を台形積分する)
         speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
@@ -325,8 +346,7 @@
         oldSpeed = speed;
 
         //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
-        //printf("%f,",speed);//速度を表示
-        printf("%f,",difference);//変位を表示
+        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示
         
         wait(0.01);
     }      
@@ -375,7 +395,12 @@
    }
             motor1.stop(0);
             motor2.stop(0);
-   
+            
+            
+            fprintf(fp, "last accel.photo\r\n");
+            fprintf(fp,"(%lf, %lf)\r\n", difference, rightrun);//最後の加速度とフォトインタラプタによる距離を出力
+            fclose(fp);
+    
 }