File content as of revision 5:9ff2f6cd54d2:

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

            pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
            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("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
             pc.printf("5m clear!");
             xbee.printf("5m clear!");
              break;
           }  
            }
       }else{
         printf("No data");
          }
        }
       return 0;      
}