修正済みby皆川
Dependencies: mbed Servo cansat_integrated_2 BMP180
Dependents: cansat_integrated_2
direction.h
- Committer:
- tsubasa_nakajima
- Date:
- 2021-10-27
- Revision:
- 1:bb89b58cfa0e
- Parent:
- 0:e7b7def631c2
- Child:
- 2:d2cb6b50a8c4
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; }