CansatB_2021
統合プログラム
Dependencies: mbed Servo BMP180
Diff: direction.cpp
- Revision:
- 8:7209c810309d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/direction.cpp Mon Nov 01 16:52:17 2021 +0000
@@ -0,0 +1,266 @@
+#include "mbed.h"
+#include "getGPS.h"
+#include "Movement.h"
+#include "direction.h"
+
+
+// 球面三角法により、大円距離(メートル)を求める
+double distance1(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 distance2 = earth_radius * rr;
+
+ return distance2;
+}
+
+
+
+//CanSatから見た目的地の角度theta(-180<=theta<=180で、角度は正面が0で反時計回り)の計算
+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(9*(y_1 - y_2)/11*(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(9*(y_0 - y_1)/11*(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(9*(y_0 - y_1)/11*(x_0 - x_1));
+ theta_1 = atan(9*(y_1 - y_2)/11*(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;
+}
+
+void direction::walk(){
+ GPS gps(D1, D0);
+ Movement idou;
+ 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;
+ idou.move_forward(20);
+ 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 = distance1(y_1,x_1,y_0,x_0);
+ d = distance1(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){
+ idou.stop();
+ break;
+ }
+
+ //移動距離が短ければ横転と判定し復帰
+ if(d < 3){
+ idou.wakeup(2);
+ idou.stop();
+ }
+
+ //Θによって回転か直進か決定
+ if(-30 < theta && theta < 30){
+ idou.move_forward(20);
+ }
+ if(theta >=30){
+ idou.turn_right(theta);
+ idou.stop();
+ idou.move_forward(20);
+ }
+ if(theta<=-30){
+ idou.turn_left(theta);
+ idou.stop();
+ idou.move_forward(20);
+ }
+
+ }
+ }
+ while(s < 2){
+ if(gps.getgps()){
+ x_1 = gps.longitude;
+ y_1 = gps.latitude;
+ idou.move_forward(20);
+ 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 = distance1(y_1,x_1,y_01,x_01);
+ d = distance1(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){
+ idou.stop();
+ break;
+ }
+
+ //移動距離が短ければ横転と判定し復帰
+ if(d < 3){
+ idou.wakeup(2);
+ idou.stop();
+ }
+
+ //thetaによって回転か直進か決定
+ if(-30 < theta && theta < 30){
+ idou.move_forward(20);
+ }
+ if(theta >=30){
+ idou.turn_right(theta);
+ idou.move_forward(20);
+ }
+ if(theta<= -30){
+ idou.turn_left(theta);
+ idou.move_forward(20);
+ }
+
+ }
+
+
+ }
+
+ idou.stop();
+}
\ No newline at end of file