ウソッキー

Dependencies:   mbed

main.cpp

Committer:
naruu
Date:
2020-12-19
Revision:
0:326c07d635d7

File content as of revision 0:326c07d635d7:

#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 (D13,D12);                 

int main(){
    double a;
    double b;
    double distance;
    
     xbee.printf("GPS begin\n");
    
    while(1){
        if(gps.getgps()){
            /*a,bを緯度経度の初期値で初期化*/
            a=gps.latitude;
            b=gps.longitude;
            xbee.printf("(%lf,%lf)\r\n",a,b);//緯度と経度を表示
            xbee.printf("--------------------------------\n\r");
                      break;
        }else{
            xbee.printf("Fault_No_Data\r\n");
         wait(1);
        }
        }  
         while(1){
         if(gps.getgps()){
             xbee.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示
            xbee.printf("--------------------------------\n\r");
             
    /*ここから距離の計算*/
                 /*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);//2点間のなす角を求める
                      double theta_r=acos(e);
                      
                      const double earth_radius=6378140;//赤道半径
                      
                      distance=earth_radius*theta_r;//距離の計算
                      
             /*距離が30m以上なら表示、通信*/         
                 if(distance>=30){
                  pc.printf("run over 20m");
                  xbee.printf("run over 20m");
                  break;
                  }

            }else {
                xbee.printf("False_No_Data\r\n");
                pc.printf("False_No_Date\r\n");
                wait(1);
            }//データ取得失敗を表示、通信、1秒待機
               
               }
    xbee.printf("成功\n");
   return 0;
   }