Diff: main.cpp

Revision:

23:6722c5f0c3f3

Parent:

22:23d9592bca04

Child:

24:b9b252fbee8e

--- a/main.cpp	Thu Dec 16 09:56:15 2021 +0000
+++ b/main.cpp	Fri Dec 17 07:24:51 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.54712081011074,34.54785702141566};  //CPリスト(x座標)
-    double CPs_y[2]={135.50777950352497,135.50680049992633};    // CPリスト(y座標)
+    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 next_CP_x, next_CP_y;
     
     // 落下検知
@@ -70,19 +70,20 @@
                 
                 catchGPS();
                 direction = AngleGet(next_CP_x,next_CP_y);
-                xbee.printf("direction=%f\n\rrotation_start", direction);
+                xbee.printf("\n\n\r----direction start-----\r\n");
+                xbee.printf("next_cp_x=%lf, next_cp_y=%lf\r\n",next_CP_x,next_CP_y);
                 //角度調節
                 while(1) {
-                    if(direction < 20 || direction > 340) {
-                        xbee.printf("direction finish\n\r");
+                    if(direction < 5 || direction > 355) {
+                        xbee.printf("\n-----direction finish-----\r\n");
                         Move('1', 0);   //停止
                         Move('2', 0.39);
-                        xbee.printf("now_angle=%f\r\n", direction);
+                        xbee.printf("now_direction=%f\r\n", direction);
                         break;
                         }
                     else {
                         Move('4', 0.19);//時計回りに回転
-                        xbee.printf("now_angle=%lf\r\n", direction);
+                        xbee.printf("now_direction=%lf\r\n", direction);
                         direction = AngleGet(next_CP_x,next_CP_y);
                         }
                 }
@@ -106,7 +107,7 @@
                 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);
+                    xbee.printf("!!!!!!!!now leach cp[%d]=x_%f,y_%f!!!!!!!!!!!\r\n",i,next_CP_x ,next_CP_y);
                     break;
                 }
                 
@@ -117,6 +118,5 @@
     Move('2', 0.17);  
     wait(0.2);
     Move('1', 0);      //停止
-    xbee.printf("mortor mode:1 speed:0");
     return 0;
 }