Diff: main.cpp

Revision:

0:44f6af5eecc5

Child:

1:8d166b7409af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 11 18:37:43 2021 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+#include"getGPS.h"
+
+
+
+Serial xbee(D1, D0);
+GPS gps(A2,A7);
+int end_flag=0;
+double bias_la=0,bias_lo=0;
+
+int main()
+{    
+    
+    xbee.printf("XBee Connected\r\n");
+    
+    while(1) {
+        
+        xbee.printf("start command\t(S or s)\r\nend command\t(E or e)");
+
+        int received_data = xbee.getc();
+        
+        xbee.printf("\r\n---------------\r\nReceived Data: %c\r\n", received_data);
+        
+        if(received_data == 83 || received_data == 115) 
+        { //S or s
+            double goal_latitude,goal_longtitude;
+            xbee.printf("Mission Start\r\n");
+            xbee.printf("please enter goal latitude:");
+            xbee.scanf("%lf",&goal_latitude);
+            xbee.printf("\r\nplease enter goal longtitude:");
+            xbee.scanf("%lf",&goal_longtitude);
+            xbee.printf("\r\n--------------\r\nstart GPS\r\n------------------\r\n");
+            end_flag=0;
+            
+            
+            while(end_flag==0)
+            {
+                if(gps.getgps())
+                { //現在地取得
+                    xbee.printf("(latitude:%lf, longtitude:%lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+                    }
+        
+                else
+                {
+                    xbee.printf("No data\r\n");//データ取得に失敗した場合
+                    }
+                
+                if(abs(gps.latitude-double(goal_latitude))<=bias_la&&abs(gps.latitude-double(goal_latitude))<=bias_lo)
+                {
+                    xbee.printf("potion match\r\n");
+                    end_flag=1;
+                }
+                if(abs(gps.latitude-double(goal_latitude))>bias_la||abs(gps.latitude-double(goal_latitude))>bias_lo)
+                {
+                    xbee.printf("\r\n-----\r\n(latitude bias:%lf,longtitude bias:%lf)\r\n",gps.latitude-double(goal_latitude),gps.latitude-double(goal_latitude));
+                }
+                wait(0.5);
+                
+            }
+            
+        
+            
+        }
+        else if(received_data == 67 || received_data == 99)
+             { //C or c
+            xbee.printf("%c\r\n", received_data);
+            xbee.printf("Mission Complete\r\n");
+            break;
+            }
+            break;
+        }
+return 0;
+        }
+        
+        
\ No newline at end of file