File content as of revision 8:8aca0c7419ab:

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

DigitalOut pin(D7);
DigitalIn pin1(D6);
PwmOut pinAFin(D10);
PwmOut pinARin(A1);
PwmOut pinBFin(D11);
PwmOut pinBRin(D12);
GPS gps(D1, D0);// tx,rx
Serial xbee(SERIAL_TX, SERIAL_RX);

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(){
    pin1.mode(PullUp);
    while(1){
        if(pin1 == 1){
            break;
            }
        }
        
    wait(50);
    pin = 1;
    wait(17);
    pin = 0;
    wait(10);
    
    
    double x,y,u,v,m,n,i,w,j,p,q,e,f,c;
    int r,s;
    u = 34.546385;
    v = 135.502085;
    s = 0;
    double t[2] = {0,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){
    }
    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に何回進むプログラムを動かすかを決める        
        t[0] = w/3.14159/0.9;
        t[1] = 2.0*j/3.0/1.2;
        xbee.printf("t[0]:%f\n",t[0]);
        xbee.printf("t[1]:%f\n",t[1]);
        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(m-x==0 && n-y==0){
            driveMotor(0,1); //直線上にある時は、18度だけ向きを変更
            wait(0.1);
            driveMotor(0,0);
            wait(3);
            }
        else if(c*i>0){
            driveMotor(0.5,0); //メモ：右回りがどっちか測定する
            wait(t[0]);
            driveMotor(0,0);
            wait(1);
            driveMotor(1,1);
            wait(t[1]);
            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);
            wait(1);
            driveMotor(1,1);
            wait(t[1]);
            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に現在の経度、緯度を記録する

        while(gps.getgps() != true){
//            xbee.printf("false\r\n");
        }
        gps.getgps();
        m=gps.latitude;
        n=gps.longitude;
                    
/*        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);
     }