修正済みby皆川
Dependencies: mbed Servo cansat_integrated_2 BMP180
Dependents: cansat_integrated_2
Diff: direction.h
- Revision:
- 2:d2cb6b50a8c4
- Parent:
- 1:bb89b58cfa0e
- Child:
- 3:a583276d9fef
--- a/direction.h Wed Oct 27 19:11:01 2021 +0000 +++ b/direction.h Wed Oct 27 20:59:02 2021 +0000 @@ -1,156 +1,119 @@ #include "mbed.h" #include "getGPS.h" -#include "Movement.h" +#include "Servo.h" +#include "math.h" -GPS gps(D1, D0); +//停止 + void stop(){ + Servo servo1(D7); +Servo servo2(A3); +Servo servo3(A1); +Servo servo4(D12); +Servo servo5(D10); +Servo servo6(A5); -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; - } + servo1 = 0.5; + servo2 = 0.5; + servo3 = 0.5; + servo4 = 0.5; + servo5 = 0.5; + servo6 = 0.5; + wait(1); } - - 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; - } + +//前進 + 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); } - - 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; - } + +//後退 + 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); } - - 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; - } - - + +//左に曲がる + 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); } - - 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; +//倒れているときの処理 + void wakeup(int time) + { + Servo servo1(D7); +Servo servo2(A3); +Servo servo3(A1); +Servo servo4(D12); +Servo servo5(D10); +Servo servo6(A5); - // 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; + int i; + for(i=1;i<=time;i++) + { + move_forward(5); + move_backward(); + turn_right(); + turn_left(); } class direction @@ -168,109 +131,116 @@ public: //歩行 - int walk(); -}; - - int direction::walk(){ - s = 0; - x_0 = NULL; - y_0 = NULL; - x_01 = NULL; - y_01 = NULL; + 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; - 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; //現在地の緯度と経度を取得 + 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); - while(1) { + //中間地点に到達したら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 - d1 = distance(y_1,x_1,y_0,x_0); - if(d1 < 15){ - Movement.stop(); + //ゴール地点に到達したらbreak + if(d2 < 15){ + stop(); break; } //移動距離が短ければ横転と判定し復帰 - d = distance(y_1,x_1,y_2,x_2); if(d < 3){ - Movement.wakeup(2); - Movement.stop(); + wakeup(2); + stop(); } - //Θによって回転か直進か決定 - theta = calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2); + //thetaによって回転か直進か決定 if(-30 < theta && theta < 30){ - Movement.move_forward(); + move_forward(); } if(theta >=30){ - Movement.turn_right(int theta); - Movement.stop(); - Movement.move_forward(); + turn_right(theta); + move_forward(); } - if(theta <= -30){ - Movement.turn_left(int theta); - Movement.stop(); - Movement.move_forward(); + if(theta<= -30){ + turn_left(theta); + 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(); - } - - } - - + stop(); } -Movement.stop(); + +}; -return 0; -} \ No newline at end of file + \ No newline at end of file