Diff: main.cpp

Revision:

6:326208aabe68

Parent:

5:cc7917e8c442

Child:

8:0f7e6ba9a434

--- a/main.cpp	Wed Nov 10 12:17:25 2021 +0000
+++ b/main.cpp	Mon Dec 06 08:30:50 2021 +0000
@@ -21,90 +21,66 @@
     // 落下検知
     // パラシュート分離
     
-    wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
+    /*wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
     while(flight_pin){}
-        pc.printf("flight_pin nuketa");
         xbee.printf("flight_pin nuketa");
         wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
         nichrome=1;
-        pc.printf("nichrome in");
         xbee.printf("nichrome in");
         wait(30);
-        nichrome=0;
+        nichrome=0;*/
     // 落下終了
     
     
     // 行動フロー開始
     Calibration();
     xbee.printf("XBee Connected\r\n");
-    pc.printf("xbee_Connected\r\n");
-    pc.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
     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まで移動
         next_CP_x = CPs_x[i];
         next_CP_y = CPs_y[i];
         
-        pc.printf("next_i=%d\r\n", i);
         xbee.printf("next_i=%d\r\n", i);
             
         while (1) {
-                speak();
+                //speak();
+                while(FrontGet()) {
+                    xbee.printf("frontget\n\r");
+                    Move('1', 0);      //停止
+                    Move('4', 0.5);    //時計回り回転
+                    wait(1);
+                    Move('1', 0);      //回転停止
+                    xbee.printf("front_avoid_rotate\n\r");
+                }
                 direction = AngleGet();
-                pc.printf("direction=%f\n\rdirection start", direction);
-                xbee.printf("direction=%f\n\rdirection start", direction);
-                int df=1;
+                xbee.printf("direction=%f\n\rrotation_start", direction);
                 //角度調節
                 while(1) {
-                    if(direction < 5 || direction > 355) {  //角度判定
-                        pc.printf("direction finish\n\r");
+                    if(direction < 20 || direction > 340) {
                         xbee.printf("direction finish\n\r");
+                        Move('1', 0);   //停止
+                        wait(5);
+                        xbee.printf("now_angle=%f\r\n", direction);
                         break;
-                    }
-                    else {
-                        Move('1', 0);//停止
-                        if(df==1){
-                        pc.printf("mortor mode:1 speed:0\n\r");
-                        xbee.printf("mortor mode:1 speed:0\n\r");
-                        }
-                        Move('4', 0.5);//時計回りに回転
-                        if(df==1){
-                        pc.printf("mortor mode:4 speed:0.5\n\r");
-                        pc.printf("mortor mode:4 speed:0.5\n\r");
-                        df++;
-                        direction = AngleGet();
                         }
-                    }
+                    else {
+                        Move('4', 0.15);//時計回りに回転
+                        xbee.printf("%now_angle=f\r\n", direction);
+                        }
                 }
-                while(FrontGet()) {
-                    pc.printf("front get\n\r");
-                    xbee.printf("frontget\n\r");
-                    Move('1', 0);      //停止
-                    pc.printf("mortor mode:1 speed:0\n\r");
-                    xbee.printf("mortor mode:1 speed:0\n\r");
-                    Move('4', 0.5);    //時計回り回転
-                    pc.printf("mortor mode:4 speed:0.5\n\r");
-                    xbee.printf("mortor mode:4 speed:0.5\n\r");
-                    wait(1);
-                    Move('1', 0);      //回転停止
-                    pc.printf("mortor mode:1 speed:0\n\r");
-                    xbee.printf("mortor mode:1 speed:0\n\r");
-                } 
-                pc.printf("flag=xbee ni Input");
+                 
                 xbee.printf("speed flag=");
                 wait(3);
-                float as;//advance speed
+                float as[2];//advance speed
                 if(xbee.readable()){
-                    pc.printf("advance speed=xbee Input");
                     xbee.printf("advance speed=");
-                    xbee.scanf("%f",&as);
+                    xbee.scanf("%f",&as[1]);
                     }else{
-                        as=0.5;
+                        as[1]=0.5;
                         }
-                Move('2', as);
-                pc.printf("mortor mode:2 speed:%f",as);
-                xbee.printf("mortor mode:2 speed:%f",as);
+                Move('2', as[1]);
+                xbee.printf("mortor mode:2 speed:%f",as[1]);
                 catchGPS();
-                pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
                 xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
                 
                 double lati = 111132.8715;    //1度あたりの緯度の距離(m)
@@ -112,7 +88,6 @@
                 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到着判定 //試験で調整
-                    pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
                     xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
                     break;
                 }
@@ -120,10 +95,8 @@
             }//while(1){}
         }//for(){}
     // 行動フロー終了
-    pc.printf("End\r\n");
     xbee.printf("End\r\n");
     Move('1', 0);      //停止
-    pc.printf("mortor mode:1 speed:0");
     xbee.printf("mortor mode:1 speed:0");
     return 0;
 }