Diff: main.cpp

Revision:

19:a1e6729a385e

Parent:

17:42f2aa7edc2b

Child:

20:da59a86eb246

--- a/main.cpp	Wed Dec 15 03:42:20 2021 +0000
+++ b/main.cpp	Wed Dec 15 09:17:48 2021 +0000
@@ -8,14 +8,14 @@
 DigitalIn flight_pin(A0);
 DigitalOut nichrome(D13);
 // 
-#define cp_max 2    //CPの数を入力する
+#define cp_max 3    //CPの数を入力する
 
 int main() {
     // 変数宣言
     double GPS_x, GPS_y;  // 現在地の座標
     double direction;     // 次CPへの向き
-    double CPs_x[2]={34.545588426585454, 34.5454484832847};  //CPリスト(x座標)
-    double CPs_y[2]={135.50282053551706, 135.50262437335775};    // CPリスト(y座標)
+    double CPs_x[3]={34.545212,34.544931,34.545007};  //CPリスト(x座標)
+    double CPs_y[3]={135.508570,135.508768,135.509057};    // CPリスト(y座標)
     double next_CP_x, next_CP_y;
     
     // 落下検知
@@ -46,6 +46,8 @@
                 speak();
                 while(FrontGet()) {
                     xbee.printf("frontget\n\r");
+                    Move('2', 0.1);
+                    wait(0.5);
                     Move('1', 0);      //停止
                     Move('4', 0.2);    //時計回り回転
                     wait(0.5);
@@ -57,10 +59,10 @@
                 
                 //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも
                 Move('2', 0.1);
-                wait(1);
+                wait(0.5);
                 Move('2', 0.2);
-                wait(2);
-                Move('2', 0.17);  
+                wait(1);
+                Move('2', 0.1);  
                 wait(0.2);
                 Move('1', 0);  
                 //ちょっと走るのおわり
@@ -73,7 +75,7 @@
                     if(direction < 20 || direction > 340) {
                         xbee.printf("direction finish\n\r");
                         Move('1', 0);   //停止
-                        Move('2', 0.5);
+                        Move('2', 0.39);
                         xbee.printf("now_angle=%f\r\n", direction);
                         break;
                         }
@@ -91,7 +93,7 @@
                     xbee.printf("advance speed=");
                     xbee.scanf("%f",&as[1]);
                     }else{
-                        as[1]=0.5;
+                        as[1]=0.39;
                         }
                 Move('2', as[1]);
                 xbee.printf("mortor mode:2 speed:%f",as[1]);