File content as of revision 1:5f317aa38c12:

#include <mbed.h>
#include <getGPS.h>

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

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;
    u = 34.546402;
    v = 135.502126;
    s = 0;
    
    xbee.printf("XBee Connected\r\n");
    while(gps.getgps() != true){
        }    
    
//基準値を記録する
    gps.getgps();
    x = gps.latitude;
    y = gps.longitude;
    xbee.printf("%f,%f\n", x, y);
//10秒前進する
    driveMotor(1,1);
    wait(10);//メモ：ただ直進してる
    driveMotor(0,0);
//m,nに現在の経度、緯度を記録する        
    gps.getgps();
    m=gps.latitude;
    n=gps.longitude;
    xbee.printf("%f,%f\n", m, n);
//a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する        
    double a[2]={m-x,n-y};
    double b[2]={u-m,v-n};
    j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
    
    
    while(j>10){

//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;
        wait(1);
        
//実際に動くときのプログラム。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);
            }
            
        //基準値を記録する
        gps.getgps();
        x = gps.latitude;
        y = gps.longitude;
//5秒前進する
        driveMotor(1,1);
        wait(5);//メモ：ただ直進してる
        driveMotor(0,0);
//m,nに現在の経度、緯度を記録する        
        gps.getgps();
        m=gps.latitude;
        n=gps.longitude;
        
        xbee.printf("%f,%f\n", x, y);
        xbee.printf("%f,%f\n", m, n);
        
//a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する        
        a[0]=m-x;
        a[1]=n-y;
        b[0]=u-m;
        b[1]=v-n;
        j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
        
        }
     }