Diff: main.cpp

Revision:

14:3e7d563538e5

Parent:

13:65ae2ea75c44

Child:

18:bd0b2394fa48

--- a/main.cpp	Sat Dec 11 01:13:30 2021 +0000
+++ b/main.cpp	Mon Dec 13 10:07:34 2021 +0000
@@ -18,56 +18,55 @@
     double CPs_y[2]={135.50282053551706, 135.50262437335775};    // CPリスト(y座標)
     double next_CP_x, next_CP_y;
     
-    // 落下検知
-    // パラシュート分離
     
-    wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
+    //落下開始
     while(flight_pin){}
-        xbee.printf("flight_pin nuketa\n");
-        wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
-        //nichrome=1;
+        xbee.printf("flight_pin nuketa\n");     // 落下検知
+        wait(5);//!ホントは35!           //ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
+        //nichrome=1;       // パラシュート分離
         xbee.printf("nichrome in\n");
         wait(10);
         nichrome=0;
-    // 落下終了
+    //着地完了
     
     
     // 行動フロー開始
     Calibration();
     xbee.printf("XBee Connected\r\n");
     xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
-    for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動
+    for (int i = 0; i<=cp_max-1 ; i++) {    //最後のcp=goalまで移動
         next_CP_x = CPs_x[i];
         next_CP_y = CPs_y[i];
         
         xbee.printf("next_i=%d\r\n", i);
             
         while (1) {
-                speak();
-                while(FrontGet()) {
-                    xbee.printf("frontget\n\r");
-                    Move('1', 0);      //停止
-                    Move('4', 0.2);    //時計回り回転
-                    wait(0.5);
-                    Move('1', 0);      //回転停止
-                    xbee.printf("front_avoid_rotate\n\r");
-                }
+            speak();
                 
+            //回避開始
+            while(FrontGet()) {
+                xbee.printf("frontget\r\n");
+                Move('1', 0);      //停止
+                Move('4', 0.2);    //時計回り回転
+                wait(0.5);
+                Move('1', 0);      //回転停止
+                xbee.printf("front_avoid_rotate\r\n");
+            }
                 //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも
                 Move('2', 0.1);
                 wait(1);
                 Move('2', 0.5);
                 wait(2);
-                Move('1', 0);  
+                Move('1', 0);       //走りながらanglegetできたら、止まらない
                 //ちょっと走るのおわり
-                //走りながらanglegetできたら、止まらない
+            //回避終了
                 
+            //角度調節開始
                 direction = AngleGet();
-                xbee.printf("direction=%f\n\rrotation_start", direction);
-                //角度調節
+                xbee.printf("direction=%f\r\nrotation_start", direction);
                 while(1) {
                     if(direction < 20 || direction > 340) {
-                        xbee.printf("direction finish\n\r");
+                        xbee.printf("rotation finish\r\n");
                         Move('1', 0);   //停止
                         Move('2', 0.5);
                         xbee.printf("now_angle=%f\r\n", direction);
@@ -76,37 +75,41 @@
                     else {
                         Move('4', 0.15);//時計回りに回転
                         xbee.printf("now_angle=%lf\r\n", direction);
+                        direction = AngleGet();
                         }
-                }
-                 
-                xbee.printf("speed flag=");
-                wait(3);
-                float as[2];//advance speed
+                }   
+            //角度調節終了
+            
+            //速度変更開始     
+            xbee.printf("speed flag=");
+            wait(3);
+            float as[2];//advance speed
                 if(xbee.readable()){
                     xbee.printf("advance speed=");
                     xbee.scanf("%f",&as[1]);
-                    }else{
-                        as[1]=0.5;
-                        }
-                Move('2', as[1]);
-                xbee.printf("mortor mode:2 speed:%f",as[1]);
-                catchGPS();
-                xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
+                }
+                else{
+                    as[1]=0.5;
+                }
+            Move('2', as[1]);
+            xbee.printf("mortor mode:2 speed:%f",as[1]);
+            catchGPS();
+            xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
+            //速度変更終了
                 
-                double lati = 111132.8715;    //1度あたりの緯度の距離(m)
-                double longi = 91535.79099;    //1度あたりの経度の距離(m)
-                GPS_x = gps.latitude;
-                GPS_y = gps.longitude;
+            //座標判定
+            double lati = 111132.8715;    //1度あたりの緯度の距離(m)
+            double longi = 91535.79099;    //1度あたりの経度の距離(m)
+            GPS_x = gps.latitude;
+            GPS_y = gps.longitude;
                 if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x)*lati*lati + (next_CP_y - GPS_y)*(next_CP_y - GPS_y)*longi*longi < 25) { // CP到着判定 //試験で調整
                     xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
                     break;
                 }
-                
-            }//while(1){}
-        }//for(){}
+        }//while(1){}
+    }//for(){}
     // 行動フロー終了
     xbee.printf("End\r\n");
-    Move('1', 0);      //停止
-    xbee.printf("mortor mode:1 speed:0");
+    Move('1', 0);
     return 0;
 }