File content as of revision 6:892eb32f6dc3:

#include "mbed.h"
#include "getGPS.h"
#include "math.h"
#include "TB6612.h"

Serial pc(USBTX,USBRX);
GPS gps (p28,p27);
Serial xbee(p13,p14);
TB6612 left(p25,p17,p16);
TB6612 right(p26,p19,p18);


int main() {
     double a;
     double b;
     double distance;
     int i = 0;
     
     pc.printf("GPS Start\n");
     xbee.printf("s\n");
     while(1) {
         if(gps.getgps()){
           
          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
          i ++;
          if(i<29){
           }else if(i == 30){
           a = gps.latitude;
           b = gps.longitude;
           
           }else{
           break;
           }
         }else{
          pc.printf("NO DATA\r\n");//データ取得失敗
          wait(1);
            }
       }
       
        left = 100; //左モーター１００％
        right = 100;//右モーター１００％
        wait(30);
        left = 0; //左モーター１０％
        right = 0;//右モーター１０％(左折)
        wait(15);
        
      while(1){
         if(gps.getgps()) {
           
          pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
          
       // 球面三角法により、大円距離(メートル)を求める
       double c;
       double d; 
        c = gps.latitude;
        d = gps.longitude;

         const double pi = 3.14159265359; // 円周率
                           
            double ra = a * pi / 180;
            double rb = b * pi / 180;     // 緯度経度をラジアンに変換
            double rc = c * pi / 180;
            double rd = d * pi / 180;
        
            double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
            double rr = acos(e);

            const double earth_radius = 6378140;   // 地球赤道半径(m)

            distance = earth_radius * rr; // 2点間の距離(m)
             

         if (distance<5){
             }else{
             pc.printf("5m clear!");
             xbee.printf("5m clear!");
             break;
              }
          
          }else{
           xbee.printf("NO DATA\r\n");//データ取得失 
           pc.printf("NO DATA\r\n");  
           wait(1);
           }
        }
        return 0;
     }