CansatB_2021
統合プログラム
Dependencies: mbed Servo BMP180
Diff: direction.h
- Revision:
- 1:bb89b58cfa0e
- Parent:
- 0:e7b7def631c2
- Child:
- 2:d2cb6b50a8c4
--- a/direction.h Thu Oct 21 01:58:35 2021 +0000
+++ b/direction.h Wed Oct 27 19:11:01 2021 +0000
@@ -1,103 +1,276 @@
#include "mbed.h"
#include "getGPS.h"
-#include "MPU9250.h"
+#include "Movement.h"
-Serial pc(SERIAL_TX, SERIAL_RX);
GPS gps(D1, D0);
-float calculation_Θ(float x_0,float y_0,float x_1,float y_1,float x,float y){
- //x,yは地磁気センサの値で北の方角の角度を90度、東の方角を0度とする。
- float Θ_0 = atan((y_0 - y_1)/(x_0 - x_1)); //目的地の角度
+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(y_0 - y_1 > 0 && x_0 - x_1 < 0){
- Θ_0 = Θ_0 - 180;
+ 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(y_0 - y_1 < 0 && x_0 - x_1 < 0){
- Θ_0 = Θ_0 + 180;
+
+ 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;
+ }
}
- float Omag_x,Omag_y; //地磁気センサのxy平面が描く円の中点
- float Θ_1 = atan((y - Omag_y)/(x- Omag_x)); //CanSatの角度
+ if(x_0 != x_1 && x_1 == x_2){
+
+ theta_0 = atan((y_0 - y_1)/(x_0 - x_1));
- if(y - Omag_y > 0 && x- Omag_x < 0){
- Θ_1 = Θ_1 - 180;
- }
- if(y - Omag_y < 0 && x - Omag_x < 0){
- Θ_1 = Θ_1 + 180;
+ 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;
+ }
}
- Θ = Θ_0 - Θ_1; //CanSatから見た目的地の角度
-}
-
-class direction()
-{
- public:
-
- Serial pc(SERIAL_TX, SERIAL_RX);
- GPS gps(D1, D0);
+ else{
+ theta_0 = atan((y_0 - y_1)/(x_0 - x_1));
+ theta_1 = atan((y_1 - y_2)/(x_1- x_2));
- float x_0 ,y_0; //目的地
-
- MPU9250 mpu9250(D4, D5); // SDA, SCL
-
- Serial pc(USBTX, USBRX, 115200);
+ 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;
+ }
- volatile bool interrupt_flag = false;
-
- void detect_shock(void)
- {
- interrupt_flag = true;
+ 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;
+ }
+
+
}
- while(1) {
- if(gps.getgps()) //現在地取得
- pc.printf("(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+ 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;
+
+ // 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;
+}
+
+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:
+
+ //歩行
+ int walk();
+};
+
+ int direction::walk(){
+ s = 0;
+ x_0 = NULL;
+ y_0 = NULL;
+ x_01 = NULL;
+ y_01 = NULL;
+ while(s<1){
+ if(gps.getgps()){
+ x_1 = gps.longitude;
+ y_1 = gps.latitude;
+ s = s + 1;
+ Movement.move_forward();
+ }
+ }
+
+ while(1) {
- else
- pc.printf("No data\r\n");//データ取得に失敗した場合
+ if(gps.getgps()){ //現在地取得
+ x_2 = x_1;
+ y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得
+ x_1 = gps.longitude;
+ y_1 = gps.latitude; //現在地の緯度と経度を取得
+
+ //中間地点に到達したらbreak
+ d1 = distance(y_1,x_1,y_0,x_0);
+ if(d1 < 15){
+ Movement.stop();
+ break;
+ }
+
+ //移動距離が短ければ横転と判定し復帰
+ d = distance(y_1,x_1,y_2,x_2);
+ if(d < 3){
+ Movement.wakeup(2);
+ Movement.stop();
+ }
+
+ //Θによって回転か直進か決定
+ 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.stop();
+ Movement.move_forward();
+ }
+ if(theta <= -30){
+ Movement.turn_left(int theta);
+ Movement.stop();
+ Movement.move_forward();
+ }
+
+ }
+
- wait(1);
+ }
+
+ 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();
+ }
+
+ }
+
+
}
-
-
- mpu9250.resetMPU9250();
- mpu9250.setWakeOnMotion();
-
- while(1)
- {
- if (interrupt_flag)
- {
-
- pc.printf("detect\r\n");
- wait_ms(1000);
- interrupt_flag = false;
- }
+Movement.stop();
- /* get mag data*/
- int16_t mag[3];
- mpu9250.readMagData(mag);
- float mag_f[3];
- for (int i = 0; i < 3; ++i)
- mag_f[i] = float(mag[i]) * mpu9250.getMres();
-
-
-
-
-
-
-
- /* serial */
-
- pc.printf("mx:%3.3f\t", mag_f[0]);
- pc.printf("my:%3.3f\t", mag_f[1]);
- pc.printf("mz:%3.3f\t\n", mag_f[2]);
-
-
- pc.printf("\n\n---------------------------\r\n");
-
- wait_ms(100);
- }
-}
-
-
\ No newline at end of file
+return 0;
+}
\ No newline at end of file