Diff: main.cpp

Revision:

0:b54d41dacf49

Child:

1:fb92cf1c694b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Dec 19 07:22:27 2020 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+#include "getGPS.h"
+#include <stdio.h>
+#include <math.h>
+
+#define PIN_SDA D4
+#define PIN_SCL D5
+
+Serial pc(SERIAL_TX,SERIAL_RX,921600);//通信
+Serial xbee(D1,D0);//Xbeeのピン
+GPS gps (D1,D0);                 
+
+
+int main()
+{
+    double a;
+    double b;
+    double distance;
+    
+    pc.printf("GPS begin\n");
+    
+    while(1){
+        if(gps.getgps()){
+            /*a,bを緯度経度の初期値で初期化*/
+            a = gps.latitude;
+            b = gps.longitude;
+            pc.printf("get1");          
+            break;
+        }else{
+            pc.printf("Fault_No_Data1\r\n");
+            wait(1);
+        }
+    }  
+    while(1){
+         if(gps.getgps()){
+            printf("get2");
+             
+    /*ここから距離の計算*/
+             /*c、dを得た緯度経度の値で初期化*/  
+            double c;
+            double d;
+            c = gps.latitude;
+            d = gps.longitude;
+                      
+            const double pi = 3.14159265359;//円周率
+                      
+             /*ラジアンに変換*/
+            double theta_a = a * pi /180;
+            double theta_b = b * pi / 180;
+            double theta_c = c * pi / 180;
+            double theta_d = d * pi / 180;
+                      
+            double e = sin(theta_a) * sin(theta_c) + cos(theta_a) * cos(theta_c) * cos(theta_b-theta_d);//２点間のなす角を求める
+            double theta_r = acos(e);
+                      
+            const double earth_radius = 6378140;//赤道半径
+                      
+            distance = earth_radius*theta_r;//距離の計算
+                      
+             /*距離が25m以上なら表示、通信*/         
+            if(distance>=30){
+                pc.printf("run over 20m");
+                break;
+            }
+
+            }else {
+                pc.printf("False_No_Data2\r\n");
+                wait(1);
+            }//データ取得失敗を表示、通信、1秒待機
+               
+            }
+    pc.printf("成功\n");
+    return 0;
+}
\ No newline at end of file