File content as of revision 5:d78746981e4f:

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

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

void driveMotor(double speedA,double 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,k;
    u = 34.546366;
    v = 135.502015;
    s = 0;
    
    xbee.printf("XBee Connected\r\n");
    while(gps.getgps() != true){
        }    
    
//基準値を記録する
    gps.getgps();
    x = gps.latitude;
    y = gps.longitude;
    xbee.printf("x:%f,y:%f\r\n", x, y);
//10秒前進する
    driveMotor(1,1);
    wait(5);//メモ：ただ直進してる
    driveMotor(0.8,0.8);
    wait(1);
    driveMotor(0.5,0.5);
    wait(2);
    driveMotor(0.2,0.2);
    wait(2);
    driveMotor(0,0);
//m,nに現在の経度、緯度を記録する
    while(gps.getgps() != true){
        xbee.printf("false\r\n");
    }
    gps.getgps();
    m=gps.latitude;
    n=gps.longitude;
    xbee.printf("m:%f,n:%f\r\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);
    xbee.printf("距離:%f\r\n", j);
    
    
    while(j>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;
        xbee.printf("i:%f\r\n",i);
        w=acos(i);
//tanの値を出す        
        e = (v-n)/(u-m);
        f = (n-y)/(m-x);
        c = (e-f)/(1+e*f);
        xbee.printf("c:%f\r\n",c);
//t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める        
        double t[2] = {w/3.14159/0.9,2*j/3/1.0}; //再度図るのは、全体の2/3だけ進んだとき
        r = t[1]/2.2;
        wait(1);
        xbee.printf("c*i:%f\r\n", c*i);
        
//実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定
        
        if(c*i>0){
            driveMotor(0.5,0); //メモ：右回りがどっちか測定する
            wait(t[0]);
            driveMotor(0,0);
            while(s == r-5){
                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++;
                }
            driveMotor(0.8,0.8);
            wait(1);
            driveMotor(0.5,0.5);
            wait(2);
            driveMotor(0.2,0.2);
            wait(2);
            driveMotor(0,0);
            }
        
        else if(c*i<0){
            driveMotor(0,0.5);
            wait(t[0]);
            driveMotor(0,0);
            while(s == r-5){
                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++;
                }
            driveMotor(0.8,0.8);
            wait(1);
            driveMotor(0.5,0.5);
            wait(2);
            driveMotor(0.2,0.2);
            wait(2);
            driveMotor(0,0);
            }
            
//0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。
        else{
            driveMotor(1,0); //直線上にある時は、18度だけ向きを変更
            wait(0.1);
            driveMotor(0,0);
            wait(3);
            }
            
        //基準値を記録する
        while(gps.getgps() != true){
//            xbee.printf("false\r\n");
        }
        gps.getgps();
        x = gps.latitude;
        y = gps.longitude;
//5秒前進する
        driveMotor(0.8,0.8);
        wait(1);
        driveMotor(0.5,0.5);
        wait(2);
        driveMotor(0.2,0.2);
        wait(2);
        driveMotor(0,0);
//m,nに現在の経度、緯度を記録する
        gps.getgps();
        m=gps.latitude;
        n=gps.longitude;
        a[0]=m-x;
        a[1]=n-y;
        k = 0;
/*        while(a[0] == 0 || a[1] == 0 || k < 5){     5mでは測れないかもと思って作ったプログラム
            driveMotor(0.5,0.5);　　　　　　　　　　　　　　内容:5mで測れなかった時５回だけ1m進むプログラム
            wait(2);
            driveMotor(0,0);
            wait(2);
            while(gps.getgps() != true){
               xbee.printf("false\r\n");
               }
            m=gps.latitude;
            n=gps.longitude;
            a[0]=m-x;
            a[1]=n-y;
            k++;
            }*/
                    
        xbee.printf("x:%f,y:%f\r\n", x, y);
        xbee.printf("m:%f,n:%f\r\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);
        
        }
     driveMotor(1,0);
     wait(3);
     driveMotor(0,0);
     }