修正済みby皆川
Dependencies: mbed Servo cansat_integrated_2 BMP180
Dependents: cansat_integrated_2
Diff: direction.h
- Revision:
- 1:bb89b58cfa0e
- Parent:
- 0:e7b7def631c2
- Child:
- 2:d2cb6b50a8c4
diff -r e7b7def631c2 -r bb89b58cfa0e direction.h --- 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