CansatB_2021
統合プログラム
Dependencies: mbed Servo BMP180
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