Revision 1:db6b55732559, committed 2021-12-03

Comitter:

ushiroji

Date:

Fri Dec 03 08:27:23 2021 +0000

Parent:

0:99f4fe3e21c6

Commit message:

first version

Changed in this revision

--- a/Function.h	Wed Dec 01 08:14:01 2021 +0000
+++ b/Function.h	Fri Dec 03 08:27:23 2021 +0000
@@ -55,7 +55,6 @@
             wait(1);
         }
     }
-    return;
 }
 
 

--- a/main.cpp	Wed Dec 01 08:14:01 2021 +0000
+++ b/main.cpp	Fri Dec 03 08:27:23 2021 +0000
@@ -1,19 +1,45 @@
 #include "mbed.h"
-// 自作関数
-#include "Function.h"
+#include "Function.h"   // 自作関数
+#define cp_max 2        //CPの数を入力する
+
 
 int main() {
-    while(1) {
-    Move('2', 0.3);
-        while(FrontGet()) {
-        xbee.printf("front get\n\r");
-        Move('1', 0);      //停止
-        xbee.printf("mortor mode:1 speed:0\n\r");
-        Move('4', 0.3);    //時計回り回転
-        xbee.printf("mortor mode:4 speed:0.5\n\r");
-        wait(1);
-        Move('1', 0);      //回転停止
-        xbee.printf("mortor mode:1 speed:0\n\r");
+    // 変数宣言
+    double GPS_x, GPS_y;  // 現在地の座標
+//    double direction;     // 次CPへの向き
+    double CPs_x[cp_max]={34.545588426585454, 34.5454484832847};  //CPリスト(x座標)
+    double CPs_y[cp_max]={135.50282053551706, 135.50262437335775};    // CPリスト(y座標)
+    double next_CP_x, next_CP_y;
+    
+    
+    catchGPS();
+    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まで移動
+        next_CP_x = CPs_x[i];
+        next_CP_y = CPs_y[i];
+        xbee.printf("next_i=%d\r\n", i);
+            while(1) {
+                Move('2', 0.3);
+    /*    
+                while(FrontGet()) {
+                Move('1', 0);      //停止
+                Move('4', 0.3);    //時計回り回転
+                wait(0.5);
+                Move('1', 0);      //回転停止
+    */
+                catchGPS();
+                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 < 10) { // CP到着判定 //試験で調整
+                        xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
+                        break;
+                    }
+            }
         }
-    }
+    xbee.printf("\n\rEnd\r\n");
+    Move('1', 0);      //停止
+    return 0;
 }

--- a/mbed.bld	Wed Dec 01 08:14:01 2021 +0000
+++ b/mbed.bld	Fri Dec 03 08:27:23 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/7c328cabac7e
\ No newline at end of file