修正済みby皆川
Dependencies: mbed Servo cansat_integrated_2 BMP180
Dependents: cansat_integrated_2
Revision 13:c482c4d7a585, committed 2021-12-19
- Comitter:
- tsubasa_nakajima
- Date:
- Sun Dec 19 02:42:53 2021 +0000
- Parent:
- 12:79946f960100
- Commit message:
- a
Changed in this revision
diff -r 79946f960100 -r c482c4d7a585 Movement.cpp --- a/Movement.cpp Fri Nov 05 15:41:24 2021 +0000 +++ b/Movement.cpp Sun Dec 19 02:42:53 2021 +0000 @@ -2,11 +2,11 @@ #include "Servo.h" #include "Movement.h" -Servo servo1(D7); -Servo servo2(A3); -Servo servo3(A1); -Servo servo4(D12); -Servo servo5(D10); +Servo servo1(A1); +Servo servo2(A3); +Servo servo3(D10); +Servo servo4(D7); +Servo servo5(D12); Servo servo6(A5); void Movement::stop(){ @@ -22,58 +22,45 @@ //前進 void Movement::move_forward(int time = 20) { - servo1 = 0; - servo2 = 0; - servo3 = 0; - servo4 = 0; - servo5 = 0; - servo6 = 0; + servo5 = 1; wait(time); } //後退 void Movement::move_backward() - { - servo1 = 1; - servo2 = 1; - servo3 = 1; - servo4 = 1; - servo5 = 1; - servo6 = 1; + { + servo1 = 0.7; + servo2 = 0.7; + servo3 = 0.7; + servo4 = 0.3; + servo5 = 0.3; + servo6 = 0.3; wait(5); } //右に曲がる void Movement::turn_right(int theta = 15) { - servo1 = 1; - servo2 = 1; - servo3 = 1; - servo4 = 0; - servo5 = 0; - servo6 = 0; + servo1 = 0.3; + servo2 = 0.3; + servo3 = 0.5; + servo4 = 0.5; + servo5 = 0.3; + servo6 = 0.3; wait(abs(theta)/15); } //左に曲がる void Movement::turn_left(int theta = 15) { - servo1 = 0; - servo2 = 0; - servo3 = 0; - servo4 = 1; - servo5 = 1; - servo6 = 1; + servo1 = 0.5; + servo2 = 0.7; + servo3 = 0.7; + servo4 = 0.7; + servo5 = 0.7; + servo6 = 0.5; wait(abs(theta)/15); } - -//倒れているときの処理 -void Movement::wakeup(int time){ - for(int i=1;i<=time;i++) - { - move_forward(5); - move_backward(); - turn_right(); - turn_left(); - } -} \ No newline at end of file + + + \ No newline at end of file
diff -r 79946f960100 -r c482c4d7a585 Movement.h --- a/Movement.h Fri Nov 05 15:41:24 2021 +0000 +++ b/Movement.h Sun Dec 19 02:42:53 2021 +0000 @@ -18,8 +18,6 @@ void turn_right(int theta); //左に曲がる void turn_left(int theta); -//横転から復帰 - void wakeup(int time); }; #endif \ No newline at end of file
diff -r 79946f960100 -r c482c4d7a585 cansat_integrated_2.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/cansat_integrated_2.lib Sun Dec 19 02:42:53 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CansatB_2021/code/cansat_integrated_2/#79946f960100
diff -r 79946f960100 -r c482c4d7a585 direction.cpp --- a/direction.cpp Fri Nov 05 15:41:24 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,285 +0,0 @@ -#include "mbed.h" -#include "getGPS.h" -#include "Movement.h" -#include "direction.h" - - -// 球面三角法により、大円距離(メートル)を求める -double distance1(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 distance2 = earth_radius * rr; - - return distance2; -} - - - -//CanSatから見た目的地の角度theta(-180<=theta<=180で、角度は正面が0で反時計回り)の計算 -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 PI = 3.14159; - 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; - } - } - - if(x_0 == x_1 && x_1 != x_2){ - theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2))*180/PI; - if(theta_1 < 0){ - theta_1 = 360 + theta_1; - } - - 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; - } - } - - if(x_0 != x_1 && x_1 == x_2){ - - theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1))*180/PI; - - if(theta_0 < 0){ - theta_0 = 360 + theta_0; - } - - - 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; - } - } - - else{ - theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1))*180/PI; - theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2))*180/PI; - - if(theta_0 < 0){ - theta_0 = 360 + theta_0; - } - - if(theta_1 < 0){ - theta_1 = 360 + theta_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 && 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 theta = theta_0 - theta_1; - - if(theta < -180){ - theta = 360 - theta; - } - if(theta > 180){ - theta = -360 + theta; - } - - return theta; -} - -void direction::walk(){ - GPS gps(D1, D0); - Movement idou; - 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; - idou.move_forward(20); - s = s + 1; - } - } - - while(1){ - if(gps.getgps()){ //現在地取得 - x_2 = x_1; - y_2 = y_1; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 - x_1 = gps.longitude; - y_1 = gps.latitude; //現在地の緯度と経度を取得 - d1 = distance1(y_1,x_1,y_0,x_0); - d = distance1(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){ - idou.stop(); - break; - } - - //移動距離が短ければ横転と判定し復帰 - if(d < 3){ - idou.wakeup(2); - idou.stop(); - } - - //Θによって回転か直進か決定 - if(-30 < theta && theta < 30){ - idou.move_forward(20); - } - if(theta >=30){ - idou.turn_left(theta); - idou.stop(); - idou.move_forward(20); - } - if(theta<=-30){ - idou.turn_right(theta); - idou.stop(); - idou.move_forward(20); - } - - } - } - while(s < 2){ - if(gps.getgps()){ - x_1 = gps.longitude; - y_1 = gps.latitude; - idou.move_forward(20); - s = s + 1; - } - } - - while(1){ - - if(gps.getgps()){ //現在地取得 - x_2 = x_1; - y_2 = y_1; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 - x_1 = gps.longitude; - y_1 = gps.latitude; //現在地の緯度と経度を取得 - d2 = distance1(y_1,x_1,y_01,x_01); - d = distance1(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){ - idou.stop(); - break; - } - - //移動距離が短ければ横転と判定し復帰 - if(d < 3){ - idou.wakeup(2); - idou.stop(); - } - - //thetaによって回転か直進か決定 - if(-30 < theta && theta < 30){ - idou.move_forward(20); - } - if(theta >=30){ - idou.turn_left(theta); - idou.move_forward(20); - } - if(theta<= -30){ - idou.turn_right(theta); - idou.move_forward(20); - } - - } - - - } - - idou.stop(); -} - \ No newline at end of file
diff -r 79946f960100 -r c482c4d7a585 direction.h --- a/direction.h Fri Nov 05 15:41:24 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,26 +0,0 @@ -#include "mbed.h" -#include "getGPS.h" -#include "Movement.h" -#include "Servo.h" - -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(); - -}; - - - \ No newline at end of file
diff -r 79946f960100 -r c482c4d7a585 getGPS.cpp --- a/getGPS.cpp Fri Nov 05 15:41:24 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,57 +0,0 @@ -#include "mbed.h" -#include "getGPS.h" - -GPS::GPS(PinName gpstx,PinName gpsrx): _gps(gpstx,gpsrx) -{ - latitude = 0; - longitude = 0; - _gps.baud(GPSBAUD); - _gps.printf("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"); -} - -bool GPS::getgps() -{ - char gps_data[256]; - int i; - - do { - while(_gps.getc() != '$'); //$マークまで読み飛ばし - i = 0; - - /* gpa_data初期化 */ - for(int j = 0; j < 256; j++) - gps_data[j] = '\0'; - - /* NMEAから一行読み込み */ - while((gps_data[i] = _gps.getc()) != '\r') { - i++; - if(i == 256) { - i = 255; - break; - } - } - } while(strstr(gps_data, "GPGGA") == NULL); //GGAセンテンスまで一行ずつ読み込み続ける - - int rlock; - char ns,ew; - double w_time, raw_longitude, raw_latitude; - int satnum; - double hdop; - - if(sscanf(gps_data, "GPGGA, %lf, %lf, %c, %lf, %c, %d, %d, %lf", &w_time, &raw_latitude, &ns, &raw_longitude, &ew, &rlock, &satnum, &hdop) > 1) { - /* 座標1(度部分) */ - int latitude_dd = (int)(raw_latitude / 100); - int longitude_dd = (int)(raw_longitude / 100); - - /* 座標2(分部分 → 度) */ - double latitude_md = (raw_latitude - latitude_dd * 100) / 60; - double longitude_md = (raw_longitude - longitude_dd * 100) / 60; - - /* 座標1 + 2 */ - latitude = latitude_dd + latitude_md; - longitude = longitude_dd + longitude_md; - - return true; - } else - return false; //GGAセンテンスの情報が欠けている時 -}
diff -r 79946f960100 -r c482c4d7a585 getGPS.h --- a/getGPS.h Fri Nov 05 15:41:24 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,19 +0,0 @@ -#ifndef GPS_H -#define GPS_H - -#define GPSBAUD 9600 //ボーレート - -class GPS { -public: - GPS(PinName gpstx,PinName gpsrx); - - bool getgps(); - - double longitude; - double latitude; - -private: - Serial _gps; -}; - -#endif //GPS_H \ No newline at end of file
diff -r 79946f960100 -r c482c4d7a585 main.cpp --- a/main.cpp Fri Nov 05 15:41:24 2021 +0000 +++ b/main.cpp Sun Dec 19 02:42:53 2021 +0000 @@ -1,7 +1,7 @@ #include "mbed.h" -#include "direction.h" #include "BMP180.h" #include "calculate.h" +#include "Movement.h" #define PIN_SDA D4 #define PIN_SCL D5 @@ -35,25 +35,25 @@ while(x<10){ if(bmp180.ReadData(&dt,&dp)){ h = calculate_h(dp0,dp,dt); - if(h >= 10){ + if(h >= 15){ x = x + 1; } wait(1); } } + wait(10); + + //離陸判定後、10秒以上高度2m以下にいた場合着地判定 + while(y<10){ + if(bmp180.ReadData(&dt,&dp)){ + h = calculate_h(dp0,dp,dt); + if(h <= 6){ + y = y + 1; + } + wait(1); + } + } - wait(10); - - //離陸判定後、10秒以上高度2m以下にいた場合着地判定 - while(y<10){ - if(bmp180.ReadData(&dt,&dp)){ - h = calculate_h(dp0,dp,dt); - if(h <= 2){ - y = y + 1; - } - wait(1); - } - } //30秒待機 wait(30); @@ -62,9 +62,30 @@ wait(10); Nichrome=0; - //中間地点を経由してゴール地点まで移動 - direction hokou; - hokou.walk(); + //前進して段差を上る、15m前進 + Movement idou; + idou.move_backward(); + wait(180); + + //右に45°方向転換して前進 + idou.turn_right(45); + idou.move_backward(); + wait(5); + + //左に45°方向転換して前進 + idou.turn_left(45); + idou.move_backward(); + wait(5); + + //右に90°方向転換して前進 + idou.turn_right(90); + idou.move_backward(); + wait(5); + + //左に90°方向転換して前進 + idou.turn_left(90); + idou.move_backward(); + wait(5); return 0;