Diff: main.cpp

Revision:

27:ffcf40b1073e

Parent:

26:7a92eef404ac

--- a/main.cpp	Fri Dec 17 08:25:06 2021 +0000
+++ b/main.cpp	Sat Dec 18 02:14:44 2021 +0000
@@ -14,8 +14,8 @@
     // 変数宣言
     double GPS_x, GPS_y;  // 現在地の座標
     double direction;     // 次CPへの向き
-    double CPs_x[3]={34.54475005065638,34.54501545998881,34.544879982295605, };  //CPリスト(x座標)
-    double CPs_y[3]={135.50554516096807,135.5058945348109, 135.50572453098422};    // CPリスト(y座標)
+    double CPs_x[cp_max]={34.54475005065638,34.54501545998881,34.544879982295605};  //CPリスト(x座標)
+    double CPs_y[cp_max]={135.50554516096807,135.5058945348109, 135.50572453098422};    // CPリスト(y座標)
     double next_CP_x, next_CP_y;
     
     // 落下検知
@@ -43,6 +43,7 @@
         xbee.printf("next_i=%d\r\n", i);
             
         while (1) {
+                int flag_avoidrun=0;
                 while(FrontGet()) {
                     xbee.printf("frontget\n\r");
                     Move('2', 0.1);
@@ -54,16 +55,19 @@
                     wait(0.2);*/
                     Move('1', 0);      //回転停止
                     xbee.printf("front_avoid_rotate\n\r");
+                    flag_avoidrun=1;
                 }
                 
                 //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも
-                Move('2', 0.1);
-                wait(0.5);
-                Move('2', 0.2);
-                wait(1);
-                Move('2', 0.15);  
-                wait(0.2);
-                Move('1', 0);  
+                if(flag_avoidrun==1){
+                    Move('2', 0.1);
+                    wait(0.5);
+                    Move('2', 0.2);
+                    wait(1);
+                    Move('2', 0.15);  
+                    wait(0.2);
+                    Move('1', 0);  
+                    }
                 //ちょっと走るのおわり
                 //走りながらanglegetできたら、止まらない