File content as of revision 1:bb89b58cfa0e:

#include "mbed.h"
#include "getGPS.h"
#include "Movement.h"

GPS gps(D1, D0);

float calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2){
    
    //float x_0 ,y_0: 目的地 , float x_1 ,y_1: 現在地, float x_2 ,y_2: 20秒前の現在地
    //theta_0:目的地の角度,theta_1:CanSatの角度theta:CanSatから見た目的地の角度(-180)   
    //theta_0,theta_1は北が90、東が0
    //theta:CanSatから見た目的地の角度(-180<=theta<=180で、角度は正面が0で反時計回り)   
    
    float theta_0,theta_1;
    
    if(x_0 == x_1 && x_1 == x_2){
        
        if(y_0 - y_1 > 0){
            theta_0 =  90;
            }
        if(y_0 - y_1 < 0){
            theta_0 = 270;
            }
        if(y_0 - y_1 == 0){
            theta_0 = 0;
            }
        
        if(y_1 - y_2 > 0){
            theta_1 =  90;
            }
        if(y_1 - y_2 < 0){
            theta_1 = 270;
            }
        if(y_1 - y_2 == 0){
            theta_1 = 0;
            }    
    }
        
    if(x_0 == x_1 && x_1 != x_2){
        theta_1 = atan((y_1 - y_2)/(x_1- x_2));  
        
        if(y_0 - y_1 > 0){
            theta_0 =  90;
            }
            
        if(y_0 - y_1 < 0){
            theta_0 = 270;
            }
            
        if(y_0 - y_1 == 0){
            theta_0 = 0;
            }
    
        if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
            theta_1 = theta_1 - 180;
        }
        if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
            theta_1 = 180;
        }
        if(y_1 - y_2 < 0 && x_1 - x_2 > 0){
            theta_1 = theta_1 + 180;
        }     
    }
    
    if(x_0 != x_1 && x_1 == x_2){
        
        theta_0 = atan((y_0 - y_1)/(x_0 - x_1));  
    
        if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
            theta_0 = theta_0 - 180;
        }
        if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
            theta_0 = 180;
        }
        if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
            theta_0 = theta_0 + 180;
        }
        
        if(y_1 - y_2 > 0){
            theta_1 =  90;
            }
        if(y_1 - y_2 < 0){
            theta_1 = 270;
            }
        if(y_1 - y_2 == 0){
            theta_1 = 0;
            }    
    }
    
    else{
        theta_0 = atan((y_0 - y_1)/(x_0 - x_1));
        theta_1 = atan((y_1 - y_2)/(x_1- x_2));  
    
        if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
            theta_0 = theta_0 - 180;
        }
        if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
            theta_0 = 180;
        }
        if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
            theta_0 = theta_0 + 180;
        }
    
        if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
            theta_1 = theta_1 - 180;
        }
        if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
            theta_1 = 180;
        }
        if(y_1 - y_2 < 0 && x_1- x_2 > 0){
            theta_1 = theta_1 + 180;
        } 
    
        
    }
    
    float theta = theta_0 - theta_1;
    if(theta < -180){
      theta = 360 - theta;
      }
    if(theta > 180){
      theta = -360 + theta;
      }  
      
    return theta;
}    

// 球面三角法により、大円距離(メートル)を求める
double distance(double lat1, double lng1, double lat2, double lng2) {

    // 円周率
    const double pi = 3.14159265359;

    // 緯度経度をラジアンに変換
    double rlat1 = lat1 * pi / 180;
    double rlng1 = lng1 * pi / 180;
    double rlat2 = lat2 * pi / 180;
    double rlng2 = lng2 * pi / 180;

    // 2点の中心角(ラジアン)を求める
    double a =
      sin(rlat1) * sin(rlat2) +
      cos(rlat1) * cos(rlat2) *
      cos(rlng1 - rlng2);
    double rr = acos(a);

    // 地球赤道半径(メートル)
    const double earth_radius = 6378140;

    // 2点間の距離(メートル)
    double distance = earth_radius * rr;

    return distance;
}

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:
    
    //歩行
    int walk();    
};

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

Movement.stop();

return 0;
}