File content as of revision 0:1b7e9aa47a80:

#include "mbed.h"
#include "math.h"

#define PI 3.14159265359

//ピンの初期化
DigitalOut motor1(p21);
DigitalOut motor2(p22);
DigitalOut motor3(p23);
DigitalOut motor4(p24);
Serial gps(p28,p27);       // tx, rx…GPSとの通信に使用
Serial pc(USBTX, USBRX);    // tx, rx
AnalogIn sw1(p15);

//グローバル変数群
int i,rlock,mode;
char gps_data[256];
char ns,ew;
float x=135.585800, y=34.648525;    //実験用初期位地
float w_time,hokui,tokei;
float g_hokui,g_tokei,old_hokui, old_tokei; //oldとg_を比べることで距離と角度を出す
float d_hokui,m_hokui,d_tokei,m_tokei;
unsigned char c;
float dis;

//プロトタイプ宣言
void Brake();
void Turn();
void Return();
void turnL();
void turnR();
void getGPS();
float distance(float ,float);
void arg(float, float);

int main() {
    
}

void Brake(){
    motor1 = 0;
    motor2 = 0;
    motor3 = 0;
    motor4 = 0;    
}

void Turn(){
    motor1 = 1;
    motor2 = 0; 
    motor3 = 1;
    motor4 = 0;     
}

void Return(){
    motor1 = 0;
    motor2 = 1;
    motor3 = 0;
    motor4 = 1;      
}

void turnL(){
    motor1 = 1;
    motor2 = 0;
    motor3 = 0;
    motor4 = 1;      
}

void turnR(){
    motor1 = 0;
    motor2 = 1;
    motor3 = 1;
    motor4 = 0;      
}

    //ヒューベニの公式で距離出す
float distance(float g_hokui, float g_tokei){
   float rlat1 = y*PI/180, rlon1 = x*PI/180;
   float rlat2 = g_hokui*PI/180, rlon2 = g_tokei*PI/180;
   float a=6378137.000, a2=pow(a,2), e=sqrt((a2-pow(6356752.314,2))/a2);
   float lat_diff = rlat1-rlat2;
   float lon_diff = rlon1-rlon2;
   float lat_ave = (rlat1+rlat2)/2;
   float w = sqrt(1-pow(e,2)*pow(sin(lat_ave),2));
   float meridian = a*(1-pow(e,2))/pow(w,3);
   float n=a/w;
   float distance2 = sqrt(pow(lat_diff*meridian,2)+pow(lon_diff*n*cos(lat_ave),2));
   pc.printf("dis2 : %fm\n\r",distance2);
   return distance2;
}

//編集中、pdfの方と比べて検討
void arg(float g_hokui, float g_tokei){
    
    int turn;
    float arg,radt,ido,keido;
    
    //緯度と経度をそれぞれの差を算出している
    ido = x - g_hokui;
    keido = y - g_tokei;
    
    arg = atan2(ido,keido);     //arctanの値を算出
    
    if(ido >= 0){
        //第一象限
        if(arg >= 0){
            radt = (arg * 180.0) / PI;
            turn = 0;   //右回り 
        }
        //第四象限
        else if(arg < 0){
            radt = ((arg + PI) * 180) /PI;
            turn = 0;
        }
    }else {
        //第二象限
        if(arg >= 0){
            radt = (arg * 180) / PI;
            turn = 1;   //左回り
        }
        //第三象限
        else if(arg < 0){
            radt = ((arg + PI) * 180) /PI;
            turn = 1;
        }
    }
}

void getGPS() {
  c = gps.getc();
  if( c=='$' || i == 256){
    mode = 0;
    i = 0;
    for(int j=0; j<256; j++){
        gps_data[j]=NULL;
    }
  }
  
  if(mode==0){
    if((gps_data[i]=c) != '\r'){
      i++;
    }else{
      gps_data[i]='\0';
      
      if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
        if(rlock==1){
          pc.printf("Status:Lock(%d)\n\r",rlock);
          
          //logitude
          d_tokei= int(tokei/100);
          m_tokei= (tokei-d_tokei*100)/60;
          g_tokei= d_tokei+m_tokei;
          //Latitude
          d_hokui=int(hokui/100);
          m_hokui=(hokui-d_hokui*100)/60;
          g_hokui=d_hokui+m_hokui;
          pc.printf("Lat:%.6f, Lon:%.6f\n\r",g_hokui, g_tokei);     //現在地のgpsデータ
          pc.printf("ch_Lat:%.6f, ch_Lon:%.6f\n\r",old_hokui-g_hokui, old_tokei-g_tokei);   //前回計測してからの移動（gps）
          //pc.printf("dis : %fm\n\r", 1000*6378.137*acos(sin(y*PI/180)*sin(g_hokui*PI/180)+cos(y*PI/180)*cos(g_hokui*PI/180)*cos(x*PI/180-g_tokei*PI/180)));   //別の距離の出し方
          dis = distance(g_hokui, g_tokei);   //ここでヒュベニの公式を利用
          old_hokui=g_hokui;
          old_tokei=g_tokei;
          
          //スイッチを押すことで目標のgpsデータ更新
          if(sw1>=0.5){
              x=g_tokei;
              y=g_hokui;
              pc.printf("\n\tchange destination\n\n\r");
            }
        }
        else{//gpsデータ未受信時
          pc.printf("\n\rStatus:unLock(%d)\n\r",rlock);
          //pc.printf("%s",gps_data);
        }
        sprintf(gps_data, "");
      }//if 
    }
  }
}