Diff: main.cpp

Revision:

3:c1e0db4832b7

Parent:

2:37d831f82840

Child:

4:8b52fd631b32

--- a/main.cpp	Thu Oct 18 02:22:16 2018 +0000
+++ b/main.cpp	Fri Oct 19 04:48:30 2018 +0000
@@ -34,17 +34,17 @@
 int main() {
     
     
-        FET = 0;                       //FET、ニクロム線切断
+      /*  FET = 0;                       //FET、ニクロム線切断
         wait(60);
         FET = 1;
         wait(30);
-        FET = 0;
+        FET = 0;   */
     
     motor1.stop(0);
     motor2.stop(0);
     
    
-    FILE *fp = fopen("/local/gps.txt", "w");  // Open "gps.txt" on the local file system for writing
+   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;
     for(n=0;n<45;n++)  //GPSの取得を45回行う(これで大体1分半)
@@ -65,37 +65,46 @@
     fclose(fp);                      //GPSの測定終了  
   
         mu.startUpdates();//start mesuring the distance(超音波センサー)
-        int distance;
+        int distance; 
         
-        while(1)
-    {
         int flag,flag2;                   //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
                                     //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
                                     //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
         float rightrun;                  //変数runをフロートで型づけする
         float leftrun2;
 
+        while(1)
+    {       
+        distance=mu.getCurrentDistance();   
+        printf("%d\r\n",distance); 
+    
+    
         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を返した時に何回も52.5mmを加算しつづけるということがなくなる
             rightrun += 52.5;                            //総距離runに52.5を加算する
+            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;
             leftrun2 += 52.5;
+            printf("test2.read else\r\n");
         }
         printf("%f", rightrun);
         printf("\t%f\r\n", leftrun2);
-       /* if (rightrun > 250){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+        if (rightrun > 250){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
             break;                                  //つまりゴールについたらこのループからぬける
-        }*/
+        }
         
         motor1.speed(0.5);   //通常走行
         motor2.speed(0.5);
@@ -103,12 +112,9 @@
                                // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
                                //the class checks if dist needs to be called.
 
-        distance=mu.getCurrentDistance();
-        wait(0.1);
+        wait(0.2);
         
-        printf("%d\r\n",distance);
-        
-        if(100<distance && distance < 500)        //障害物発見❕
+        if(100 < distance && distance < 500)        //障害物発見❕
          {
             
             printf("if success!\r\n");
@@ -129,16 +135,31 @@
             motor2.speed(-msj2);
             wait(2);
             printf("mortor rotation\r\n");
+            
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(2);
+            printf("mortor stop\r\n");
 
             motor1.speed(ms1);       //直進
             motor2.speed(ms2);
             wait(2);
             printf("mortor straight\r\n");
+            
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(2);
+            printf("mortor stop\r\n");
 
             motor1.speed(-msj1);      //機体を反時計回りに９０度回転
             motor2.speed(msj2);
             wait(2);
             printf("mortor rotation\r\n");
+            
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(2);
+            printf("mortor stop\r\n");
 
             motor1.speed(ms1);       //直進
             motor2.speed(ms2);
@@ -163,22 +184,42 @@
         printf("%f", rightrun);
         printf("\t%f\r\n", leftrun2);*/
             wait(2);
-            printf("mortor straight\r\n");            
+            printf("mortor straight\r\n");   
+            
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(2);
+            printf("mortor stop\r\n");         
 
             motor1.speed(-msj1);      //機体を反時計回りに９０度回転
             motor2.speed(msj2);
             wait(2);
             printf("mortor rotation\r\n");
+            
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(2);
+            printf("mortor stop\r\n");
 
             motor1.speed(ms1);       //直進
             motor2.speed(ms2);
             wait(2);
             printf("mortor straight\r\n");
+            
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(2);
+            printf("mortor stop\r\n");
 
             motor1.speed(msj1);       //機体を時計回りに９０度回転
             motor2.speed(-msj2);
             wait(2);   
             printf("mortor rotation\r\n");
+            
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(2);
+            printf("mortor stop\r\n");
         }
    }
 }