Diff: main.cpp

Revision:

1:edc264954800

Parent:

0:6212b283430c

Child:

2:e2b803e3bcbc

diff -r 6212b283430c -r edc264954800 main.cpp
--- a/main.cpp	Sun Oct 27 05:20:43 2019 +0000
+++ b/main.cpp	Fri Nov 01 15:02:54 2019 +0000
@@ -129,369 +129,61 @@
    fprintf(fp,“GPS finish\r\n”);
   // fclose(fp);                     // GPSの測定終了   */
 
-if
+if(distance<4000){
+    motorStopR();
+    motorStopL();
+    stopUS015();
+    startsb612a();
 //超音波センサー反応あり
 //停止
 //超音波センサーOFF
 //焦電センサーON
-   if
+   if(detected=true){
+       stopsb612a();
+       startCamera();
+       stopCamera();
+       sendSD();
+       sendPC();
+       
    //焦電センサー反応あり
    //焦電センサーOFF
    //カメラ起動
-   //撮影
    //カメラOFF
    //データをSDカードに保存
    //保存データをXBeeによりPCへ送信
-       if
+       if(receiveOK()=true){
+           printf("Unknown Creature has been descovered!\n");
+           }
        //"OK"受信、ミッション終了
-       else if
+       else if(receiveOK()=false){
+           printf("Continue!\n");
+           }
        //"NO"受信、ミッション再開
 
-    else if
+    else if(detected=false){
+        stopsb612a();
+        motorForwardL();
+        motorStopL();
+        motorForwardR();
+        motorForwardL();
+        startUS015();
+        }
+    } 
     //焦電センサー反応無し
     //焦電センサーOFF
     //方向転換
     //超音波センサーON
     //直進
-else if
+}
+else if(distance>=4000){
+    motorStopL();
+    motorStopR();
+    motorForwardL();
+    motorStopL();
+    motorForwardR();
+    motorForwardL();
+}
 //超音波センサー反応無し
 //停止
 //方向転換
 //直進
-
-
-
-
-
-
-double heading,wx,wy,wz;
-//   mcn1=1.0;
-  // mcn2=1.0;
-heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
-if(90<heading<267.5){
-   printf(“right\r\n”);
-   motorForwardL();//右回転
-   motorReverseR();
-   wx=(270-heading)*0.004448;
-   wait(wx); //角度のずれ*1度回転するのにかかる時間
-   motorStopR();
-   motorStopL();
-   wait(1);
-}
-else if(0<=heading<=90.0){
-   printf(“left\r\n”);
-   motorForwardL();//左回転
-   motorReverseR();
-   wy=(270-heading)*0.004448;
-   wait(wy);
-   motorStopR();
-   motorStopL();
-   wait(1);
-}
-else if(272.5<heading<360){
-   printf(“left\r\n”);
-   motorForwardL();//左回転
-   motorReverseR();
-   wz=(360-heading)*0.004448;
-   wait(wz);
-   motorStopR();
-   motorStopL();
-   wait(1);
-}
-else{
-   wait(5);
-}
-printf(“searchN\r\n”); //機体が北を向く
-wait(2);
-       mu.startUpdates();//start mesuring the distance(超音波センサー)
-       int distance;
-       int flag=0,flag2=0;                   //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
-                                   //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
-                                   //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
-       float rightrun;                  //変数runをフロートで型づけする
-       float leftrun;
-       rightrun=0.0;
-       leftrun=0.0;
-       int accel[3];//accelを3つの配列で定義。*/
-       int gyro[3];
-       int tt=0;
-       float run=0;
-       fprintf(fp, “x,y,z,lefttrun2,rightrun\r\n”);
-       int k=0;//中間地点角度判定プログラムを１回しか通さないためのフラグ
-       while(1)
-   {
-       distance=mu.getCurrentDistance();
-       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軸方向の加速度
-       mpu.readGyroData(gyro);//加速度の値をaccel[3]に代入
-       int gx = gyro[0];//x軸方向の加速度
-       int gy = gyro[1];//y軸方向の加速度
-       int gz = gyro[2];//z軸方向の加速度
-       printf(“x=%d,y=%d,z=%d\r\n”,x,y,z); //加速度の表示
-       printf(“%d\r\n”, test.read());              //フォトインタラプタ
-       printf(“%d\r\n”, test2.read());
-       if (test.read() == 1 and flag == 0){        //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
-           flag = 1;                               //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
-           printf(“test.read if\r\n”);
-       }
-       else if (test.read() == 0 and flag == 1){   //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
-           flag = 0;                               //まずこれでスイッチを0にして入れる。
-                                                   //こうすることで同じスリットの中でtestが複数回0を返した時に何回も56.5mmを加算しつづけるということがなくなる
-           rightrun += 113;                            //総距離runに113mmを加算する(タイヤの全周452mm)
-           printf(“test.read else\r\n”);
-       }
-        if (test2.read() == 1 and flag2 == 0){
-           flag2 = 1;
-           printf(“test2.read if\r\n”);
-       }
-       else if (test2.read() == 0 and flag2 == 1){
-           flag2 = 0;
-           leftrun += 113;
-           printf(“test2.read else\r\n”);
-       }
-       printf(“%f %g rot “, leftrun,leftrun/452);
-       printf(“%f %g rot\r\n”, rightrun,rightrun/452);
-       run=culculate_distance_3(rightrun,leftrun);
-if(run >= 30000){//もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
-       break;                                  //つまりゴールについたらこのループからぬける
-       }
-   else if(15000< run && run<15050 && k!=1){//中間地点で北を向きなおす
-   motorStopR();
-   motorStopL();
-   wait(1);
-   heading=compass.getHeadingXYDeg();//コンパスの値をもう一度headingに格納
-   wait(2);
-   k=1;
-heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
-if(90<heading<267.5){
-   printf(“right\r\n”);
-   motorForwardL();//右回転
-   motorReverseR();
-   wx=(270-heading)*0.004448;
-   wait(wx); //角度のずれ*1度回転するのにかかる時間
-   motorStopR();
-   motorStopL();
-   wait(1);
-}
-else if(0<=heading<=90.0){
-   printf(“left\r\n”);
-   motorForwardL();//左回転
-   motorReverseR();
-   wy=(270-heading)*0.004448;
-   wait(wy);
-   motorStopR();
-   motorStopL();
-   wait(1);
-}
-else if(272.5<heading<360){
-   printf(“left\r\n”);
-   motorForwardL();//左回転
-   motorReverseR();
-   wz=(360-heading)*0.004448;
-   wait(wz);
-   motorStopR();
-   motorStopL();
-   wait(1);
-}
-else{
-   wait(5);
-}
-printf(“searchN\r\n”); //機体が北を向く
-}
-   /*    if(difference >= 0.3)
-        {
-            break;
-        }    */
-       motorForwardL();   //通常走行
-       motorForwardR();
-                              //Do something else here
-                              // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
-                              //the class checks if dist needs to be called.
-       wait(0.01);
-           fprintf(fp,“%5d, %5d, %5d, %5d, %5d, %5d, %8lf, %8lf\r\n”, x, y, z,gx,gy,gz,  leftrun, rightrun);//加速度とフォトインタラプタによる距離を出力
-       if(450 < distance && distance < 550)        //障害物発見
-        {
-           printf(“if success!\r\n”);
-           float ms1,ms2,msj1,msj2;
-           ms1=1.0;            //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ
-           ms2=1.0;
-           msj1=1.0;           //回転の時
-           msj2=1.0;
-           motorStopR();
-           motorStopL();
-           wait(2);
-           printf(“mortor stop\r\n”);
-           motorForwardL();       //機体を時計回りに９０度回転
-           motorReverseR();
-           wait(1);
-           printf(“mortor rotation\r\n”);
-           motorStopR();
-           motorStopL();
-           wait(2);
-           printf(“mortor stop\r\n”);
-           motorForwardL();       //直進
-           motorForwardR();
-           wait(3);
-           printf(“mortor straight\r\n”);
-           motorStopR();
-           motorStopL();
-           wait(2);
-           printf(“mortor stop\r\n”);
-           motorReverseL();      //機体を反時計回りに９０度回転
-           motorForwardR();
-           wait(1);
-           printf(“mortor rotation\r\n”);
-           motorStopR();
-           motorStopL();
-           wait(2);
-           printf(“mortor stop\r\n”);
-           motorForwardL();       //直進
-           motorForwardR();
-       int t=0;
-   for(t=0;t<100;t++)
-   {
-           printf(“%d\r\n”, test.read());
-           printf(“%d\r\n”, test2.read());
-           if (test.read() == 1 and flag == 0)
-           {
-               flag = 1;
-           }
-           else if (test.read() == 0 and flag == 1)
-           {
-               flag=0;
-               rightrun += 113;
-           }
-            if (test2.read() == 1 and flag2 == 0){
-           flag2 = 1;
-       }
-       else if (test2.read() == 0 and flag2 == 1){
-           flag2 = 0;
-           leftrun += 113;
-       }
-       printf(“%f”, leftrun);
-       printf(“\t%f\r\n”, rightrun);
-       mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
-       mpu.readGyroData(gyro);
-       int x = accel[0];
-       int y = accel[1];
-       int z = accel[2];
-       int gx = gyro[0];
-       int gy = gyro[1];
-       int gz = gyro[2];
-fprintf(fp,“%5d, %5d, %5d, %8lf, %8lf, %5d, %5d, %5d\r\n”, x, y, z, leftrun, rightrun,gx, gy ,gz);//加速度とフォトインタラプタによる距離を出力
-      if (run >= 300000){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
-           break;                                  //つまりゴールについたらこのループからぬける
-       }
-       wait(0.01);
-   }
-           printf(“mortor straight\r\n”);
-           motorStopR();
-           motorStopL();
-           wait(2);
-           printf(“mortor stop\r\n”);
-           motorReverseL();      //機体を反時計回りに９０度回転
-           motorForwardR();
-           wait(1);
-           printf(“mortor rotation\r\n”);
-           motorStopR();
-           motorStopL();
-           wait(2);
-           printf(“mortor stop\r\n”);
-           motorForwardL();       //直進
-           motorForwardR();
-           wait(2);
-           printf(“mortor straight\r\n”);
-           motorStopR();
-           motorStopL();
-           wait(2);
-           printf(“mortor stop\r\n”);
-           motorForwardL();       //機体を時計回りに９０度回転
-           motorReverseR();
-           wait(1);
-           printf(“mortor rotation\r\n”);
-           motorStopR();
-           motorStopL();
-           wait(2);
-           printf(“mortor stop\r\n”);
-heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
-if(90<heading<267.5){
-   printf(“right\r\n”);
-   motorForwardL();//右回転
-   motorReverseR();
-   wx=(270-heading)*0.004448;
-   wait(wx); //角度のずれ*1度回転するのにかかる時間
-   motorStopR();
-   motorStopL();
-   wait(1);
-}
-else if(0<=heading<=90.0){
-   printf(“left\r\n”);
-   motorForwardL();//左回転
-   motorReverseR();
-   wy=(270-heading)*0.004448;
-   wait(wy);
-   motorStopR();
-   motorStopL();
-   wait(1);
-}
-else if(272.5<heading<360){
-   printf(“left\r\n”);
-   motorForwardL();//左回転
-   motorReverseR();
-   wz=(360-heading)*0.004448;
-   wait(wz);
-   motorStopR();
-   motorStopL();
-   wait(1);
-}
-else{
-   wait(5);
-}
-printf(“searchN\r\n”); //機体が北を向く
-       }
-  }
-           motorStopR();
-           motorStopL();
-           fprintf(fp, “last photo\r\n”);
-           fprintf(fp,“(%lf)\r\n”, run);//最後の加速度とフォトインタラプタによる距離を出力
-           fprintf(fp, “last left.right\r\n”);
-           fprintf(fp,“(%lf, %lf)\r\n”, leftrun, rightrun);
-           fclose(fp);
-}
-        float culculate_distance_3(float a,float b)        //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
-           {
-              float c;
-              c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
-              return c;
-           }
-void motorForwardR() {
-   motorStopR();
-   motor1Dir1 = 1;
-   motor1Dir2 = 0;
-}
-void motorReverseR() {
-   motorStopR();
-   motor1Dir1 = 0;
-   motor1Dir2 = 1;
-}
-void motorStopR() {
-   motor1Dir1 = 0;
-   motor1Dir2 = 0;
-}
-void motorForwardL() {
-   motorStopR();
-   motor2Dir1 = 1;
-   motor2Dir2 = 0;
-}
-void motorReverseL() {
-   motorStopR();
-   motor2Dir1 = 0;
-   motor2Dir2 = 1;
-}
-void motorStopL() {
-   motor2Dir1 = 0;
-   motor2Dir2 = 0;
-}