統合プログラム
Dependencies: mbed Servo BMP180
direction.h
- Committer:
- tsubasa_nakajima
- Date:
- 2021-10-28
- Revision:
- 5:e1001bfc423a
- Parent:
- 3:a583276d9fef
- Child:
- 6:6fe6e3554a46
File content as of revision 5:e1001bfc423a:
#include "mbed.h" #include "getGPS.h" #include "Servo.h" #include "math.h" //停止 void stop(){ Servo servo1(D7); Servo servo2(A3); Servo servo3(A1); Servo servo4(D12); Servo servo5(D10); Servo servo6(A5); servo1 = 0.5; servo2 = 0.5; servo3 = 0.5; servo4 = 0.5; servo5 = 0.5; servo6 = 0.5; wait(1); } //前進 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); } //後退 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); } //左に曲がる 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); } //倒れているときの処理 void wakeup(int time) { Servo servo1(D7); Servo servo2(A3); Servo servo3(A1); Servo servo4(D12); Servo servo5(D10); Servo servo6(A5); int i; for(i=1;i<=time;i++) { move_forward(5); move_backward(); turn_right(); turn_left(); } 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: //歩行 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; } } 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); //中間地点に到達したら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 if(d2 < 15){ stop(); break; } //移動距離が短ければ横転と判定し復帰 if(d < 3){ wakeup(2); stop(); } //thetaによって回転か直進か決定 if(-30 < theta && theta < 30){ move_forward(); } if(theta >=30){ turn_right(theta); move_forward(); } if(theta<= -30){ turn_left(theta); move_forward(); } } } stop(); } };