File content as of revision 2:591877e2c7d1:

#include <mbed.h>
#include <getGPS.h>
//イレギュラーへの対応がまだできてません

PwmOut pinAFin(A1);
PwmOut pinARin(D10);
PwmOut pinBFin(D11);
PwmOut pinBRin(D12);
Serial pc(SERIAL_TX, SERIAL_RX);
GPS gps(D1, D0);// tx,rx

void driveMotor(int speedA,int speedB) {
  float outputA = abs(speedA);
  float outputB = abs(speedB);
  if (speedA > 0) {
    pinAFin=outputA;
    pinARin=0;
  } else if (speedA < 0) {
    pinAFin=0;
    pinARin=outputA;
  } else {
    pinAFin=0;
    pinARin=0;
  }
  if (speedB > 0) {
    pinBFin=outputB;
    pinBRin=0;
  } else if (speedB < 0) {
    pinBFin=0;
    pinBRin=outputB;
  } else {
    pinBFin=0;
    pinBRin=0;
  }
}

int main(){
    double x, y;
    double m, n;
    double i,w;
    gps.getgps();
    x = gps.latitude; //経度の初期値
    y = gps.longitude; //緯度の初期値
    while(1){
        driveMotor(1,1);
        wait(30);
        driveMotor(0,0);
        m=gps.latitude; //現在の経度
        n=gps.longitude; //現在の緯度
        x=m-x;
        y=n-y;
        double a[2]={x,y};
        double b[2]={0-m,0-n}; //目的地の座標を(0,0)とした
        double p=a[0]*a[0]+a[1]*a[1];
        double q=b[0]*b[0]+b[1]*b[1];
        double pp=pow(p,0.5);
        double qq=pow(q,0.5);
        i=(a[0]*b[0]+a[1]*b[1])/pp*qq;
        w=acos(i);
        double e = (0-n)/(0-m);//目的地の座標を(0,0)とした
        double f = (n-y)/(m-x);
        double c = (e-f)/(1+e*f);
        double d = m*m+n*n;
        double dd = pow(d,0.5); //目的地までの距離
        double t[2] = {5*w/3.14159,2*dd/3*1}; //方向転換は5秒以内、1はモーターの1m/sを緯度、経度に変換した値。再度図るのは、全体の2/3だけ進んだときにしました。
        if(c*w>0){
            driveMotor(1,0); //方向転換、単位は1秒あたり36度。
            wait(t[0]);
            driveMotor(0,0);
            wait(0.5);
            driveMotor(1,1);
            wait(t[1]);
            }
        else if(c*w<0){
            driveMotor(0,1);
            wait(t[0]);
            driveMotor(0,0);
            wait(0.5);
            driveMotor(1,1);
            wait(t[1]);
            }
        else{
            return 0;
            }
        }
     }