File content as of revision 2:d2cb6b50a8c4:

#include "mbed.h"
#include "getGPS.h"
#include "Servo.h"
#include "math.h"

//停止
   void stop(){
        Servo servo1(D7);
Servo servo2(A3);  
Servo servo3(A1);
Servo servo4(D12);
Servo servo5(D10);
Servo servo6(A5);

        servo1 = 0.5;
        servo2 = 0.5;
        servo3 = 0.5;
        servo4 = 0.5;
        servo5 = 0.5;
        servo6 = 0.5;
        wait(1);
    }

//前進
    void move_forward(int time = 20)
    {
        Servo servo1(D7);
Servo servo2(A3);  
Servo servo3(A1);
Servo servo4(D12);
Servo servo5(D10);
Servo servo6(A5);

        servo1 = 0;
        servo2 = 0;
        servo3 = 0;
        servo4 = 0;
        servo5 = 0;
        servo6 = 0;
        wait(time);
    }

//後退
    void move_backward()
    {
        Servo servo1(D7);
Servo servo2(A3);  
Servo servo3(A1);
Servo servo4(D12);
Servo servo5(D10);
Servo servo6(A5);

        servo1 = 1;
        servo2 = 1;
        servo3 = 1;
        servo4 = 1;
        servo5 = 1;
        servo6 = 1;
        wait(5);
    }

//右に曲がる
    void turn_right(int theta = 15)
    {
        Servo servo1(D7);
Servo servo2(A3);  
Servo servo3(A1);
Servo servo4(D12);
Servo servo5(D10);
Servo servo6(A5);

        servo1 = 1;
        servo2 = 1;
        servo3 = 1;
        servo4 = 0;
        servo5 = 0;
        servo6 = 0;
        wait(theta/15);
    }

//左に曲がる
    void turn_left(int theta = 15)
    {
        Servo servo1(D7);
Servo servo2(A3);  
Servo servo3(A1);
Servo servo4(D12);
Servo servo5(D10);
Servo servo6(A5);

        servo1 = 0;
        servo2 = 0;
        servo3 = 0;
        servo4 = 1;
        servo5 = 1;
        servo6 = 1;
        wait(theta/15);
    }

//倒れているときの処理
    void wakeup(int time)
    {
        Servo servo1(D7);
Servo servo2(A3);  
Servo servo3(A1);
Servo servo4(D12);
Servo servo5(D10);
Servo servo6(A5);

        int i;
        for(i=1;i<=time;i++)
        {
        move_forward(5);
        move_backward();
        turn_right();
        turn_left();
}

class direction
{   
    private:
    
    int s;
    float x_0 ,y_0; //中間地点の座標(未定)
    float x_01,y_01; //ゴール地点の座標(未定)
    float x_1 ,y_1; //現在地
    float x_2 ,y_2; //20秒前の現在地
    float theta;   //CanSatから見た目的地の角度
    float d,d1,d2;  //dはCanSatの20秒間の移動距離,d1はCanSatと中間地点の距離、d2はゴール地点との距離(単位はm)
    
    public:
    
    //歩行
    void walk(){
        GPS gps(D1, D0);
        s = 0; 
        x_0 = 0;
        y_0 = 0;
        x_01 = 0;
        y_01 = 0;
        
        while(s<1){
        if(gps.getgps()){
            x_1 = gps.longitude; 
            y_1 = gps.latitude;
            move_forward();
            s = s + 1;
            }
        }    
        
       while(1){        
        if(gps.getgps()){ //現在地取得
            x_2 = x_1;           
            y_2 = y_2;           //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得
            x_1 = gps.longitude; 
            y_1 = gps.latitude; //現在地の緯度と経度を取得
            d1 = distance(y_1,x_1,y_0,x_0);
            d = distance(y_1,x_1,y_2,x_2);
            theta =  calculate_theta(x_0,y_0,x_1,y_1,x_2,y_2);
            
            //中間地点に到達したらbreak            
            if(d1 < 15){
                stop();
                break;
                } 
            
            //移動距離が短ければ横転と判定し復帰   
            if(d < 3){
                wakeup(2);
                stop();
            } 
            
            //Θによって回転か直進か決定
            if(-30 < theta && theta < 30){
                move_forward();
                }
            if(theta >=30){
                turn_right(theta);
                stop();
                move_forward();
                }
            if(theta<=-30){
                turn_left(theta);
                stop();
                move_forward();
                }        
                            
        }
    }
        while(s < 2){
            if(gps.getgps()){
            x_1 = gps.longitude; 
            y_1 = gps.latitude;
            move_forward();
            s = s + 1;
            }  
        }
    
    while(1){
        
        if(gps.getgps()){ //現在地取得
            x_2 = x_1;           
            y_2 = y_2;           //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得
            x_1 = gps.longitude; 
            y_1 = gps.latitude; //現在地の緯度と経度を取得
            d2 = distance(y_1,x_1,y_01,x_01);
            d = distance(y_1,x_1,y_2,x_2);
            theta =  calculate_theta(x_01,y_01,x_1,y_1,x_2,y_2);
            
            //ゴール地点に到達したらbreak
            if(d2 < 15){
                stop();
                break;
                } 
            
            //移動距離が短ければ横転と判定し復帰   
            if(d < 3){
                wakeup(2);
                stop();
            } 
            
            //thetaによって回転か直進か決定
            if(-30 < theta && theta < 30){
                move_forward();
                }
            if(theta >=30){
                turn_right(theta);
                move_forward();
                }
            if(theta<= -30){
                turn_left(theta);
                move_forward();
                }        
                            
        }
    
        
    } 
    
    stop();  
    }

  
};