統合プログラム
Dependencies: mbed Servo BMP180
Diff: direction.h
- Revision:
- 5:e1001bfc423a
- Parent:
- 3:a583276d9fef
- Child:
- 6:6fe6e3554a46
diff -r 6e24a1b3edca -r e1001bfc423a direction.h --- a/direction.h Thu Oct 28 08:31:00 2021 +0000 +++ b/direction.h Thu Oct 28 09:12:27 2021 +0000 @@ -1,154 +1,7 @@ #include "mbed.h" #include "getGPS.h" #include "Servo.h" - -// 球面三角法により、大円距離(メートル)を求める -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; -} - -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; -} +#include "math.h" //停止 void stop(){