File content as of revision 0:c51bac912336:

#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,u,v,m,n,i,w,j,p,q,e,f,c;
    int r,s;
    gps.getgps();
    x = gps.latitude;
    y = gps.longitude;
    while(j>10){
//30秒前進する
        driveMotor(1,1);
        wait(30);//メモ：ただ直進してる
        driveMotor(0,0);
//m,nに現在の経度、緯度を記録する        
        gps.getgps();
        m=gps.latitude;
        n=gps.longitude;
//a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する        
        double a[2]={m-x,n-y};
        double b[2]={u-m,v-n};
        j=pow(b[0]*b[0]/0.000008983148616/0.000008983148616+b[1]*b[1]/0.000010966382364/0.000010966382364,0.5);
//cosと角度の値を出す        
        p=pow(a[0]*a[0]+a[1]*a[1],0.5);
        q=pow(b[0]*b[0]+b[1]*b[1],0.5);
        i=(a[0]*b[0]+a[1]*b[1])/p*q;
        w=acos(i);
//tanの値を出す        
        e = (v-n)/(u-m);
        f = (n-y)/(m-x);
        c = (e-f)/(1+e*f);
//t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める        
        double t[2] = {w/3.14159/2,2*j/3/1.2}; //再度図るのは、全体の2/3だけ進んだとき
        r = t[1]/2.2;
        
//実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定
        
        if(c*i>0){
            driveMotor(1,0); //メモ：右回りがどっちか測定する
            wait(t[0]);
            driveMotor(0,0);
            while(s == r){
                driveMotor(1,1);
                wait(1.8);
                driveMotor(0,0);
//超音波センサの起動。首を振る
                driveMotor(0,1);
                wait(0.1);
                driveMotor(1,0);
                wait(0.2);
                driveMotor(0,1);
                wait(0.1);
//超音波センサ終了
                s++;
                }
            }
        
        else if(c*i<0){
            driveMotor(0,1);
            wait(t[0]);
            driveMotor(0,0);
            while(s == r){
                driveMotor(1,1);
                wait(1.8);
                driveMotor(0,0);
//超音波センサの起動。首を振る
                driveMotor(0,1);
                wait(0.1);
                driveMotor(1,0);
                wait(0.2);
                driveMotor(0,1);
                wait(0.1);
//超音波センサ終了
                s++;
                }
            }
            
//0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。
        else{
            driveMotor(1,0); //直線上にある時は、18度だけ向きを変更
            wait(0.1);
            driveMotor(0,0);
            }
        }
     }