Diff: main.cpp

Revision:

20:02afaa2186f3

Parent:

19:046524ac0985

--- a/main.cpp	Thu Oct 28 13:16:07 2021 +0000
+++ b/main.cpp	Thu Nov 04 11:35:18 2021 +0000
@@ -15,8 +15,8 @@
     // 変数宣言
     double GPS_x, GPS_y;  // 現在地の座標
     double direction;     // 次CPへの向き
-    double CPs_x[3]={34.545658, 34.545837, 34.546052};  //CPリスト(x座標)
-    double CPs_y[3]={135.503469, 135.503676, 135.503503};    // CPリスト(y座標)
+    double CPs_x[3]={34.54608391847546, 34.545845666047306, 34.545666059919796};  //CPリスト(x座標)
+    double CPs_y[3]={135.50338843400897, 135.50368659080985, 135.50347298593758};    // CPリスト(y座標)
     double next_CP_x, next_CP_y;
     
     // 落下検知
@@ -35,6 +35,7 @@
     
     
     // 行動フロー開始
+    Calibration();
     while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定
         for (int i = 0; i<=cp_max-1 ; i++) {
             // 移動
@@ -42,7 +43,6 @@
         next_CP_y = CPs_y[i];
         
         pc.printf("i=%d\r\n", i);
-        catchGPS();
         pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
             
             while (1) {
@@ -73,7 +73,10 @@
                 }
                 catchGPS();
                 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
-                if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x) + (next_CP_y = GPS_y)*(next_CP_y = GPS_y) < 0.01) { // CP到着判定 //試験で調整
+                
+                double lati = 111132.8715;    //1度あたりの緯度の距離(m)
+                double longi = 91535.79099;    //1度あたりの経度の距離(m)
+                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 < 100) { // CP到着判定 //試験で調整
                     pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
                     break;
                 }