気球試験1,2回目 3回目はtest2-xbee
Dependencies: ATP3012 mbed a HMC US015_2 getGPS
main.cpp
- Committer:
- miyajitakenari
- Date:
- 2021-12-19
- Revision:
- 0:e386cbba36d5
File content as of revision 0:e386cbba36d5:
/*ライブラリ*/ #include "mbed.h" // 自作関数 #include "Function.h" // フライトピン・ニクロム線関係 DigitalIn flight_pin(A0); DigitalOut nichrome(D13); // #define cp_max 4 //CPの数を入力する int main() { // 変数宣言 double GPS_x, GPS_y; // 現在地の座標 double direction; // 次CPへの向き double CPs_x[cp_max]={34.54497251645555,34.545054367534235,34.54502498510097,34.545342036897935}; //CPリスト(x座標) double CPs_y[cp_max]={135.50877273927074,135.50837525002106,135.50798030878718,135.50778889521388}; // CPリスト(y座標) double next_CP_x, next_CP_y; // 落下検知 // パラシュート分離 wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん while(flight_pin){} xbee.printf("flight_pin nuketa\n\r"); wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s nichrome=1;// ここ変える xbee.printf("nichrome in\n\r"); wait(10); nichrome=0; // 落下終了 // 行動フロー開始 Calibration(); xbee.printf("XBee Connected\r\n"); xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude); for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動 next_CP_x = CPs_x[i]; next_CP_y = CPs_y[i]; xbee.printf("next_i=%d\r\n", i); while (1) { while(FrontGet()) { xbee.printf("frontget\n\r"); Move('2', 0.1); wait(0.5); Move('1', 0); //停止 Move('4', 0.2); //時計回り回転 wait(0.5); /*Move('2', 0.17); wait(0.2);*/ Move('1', 0); //回転停止 xbee.printf("front_avoid_rotate\n\r"); } //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも Move('2', 0.2); wait(0.8); Move('1', 0); //ちょっと走るのおわり //走りながらanglegetできたら、止まらない catchGPS(); direction = AngleGet(next_CP_x,next_CP_y); xbee.printf("\n\n\r----direction start-----\r\n"); xbee.printf("next_cp_x=%lf, next_cp_y=%lf\r\n",next_CP_x,next_CP_y); //角度調節 while(1) { if(direction < 5 || direction > 350) { xbee.printf("\n-----direction finish-----\r\n"); Move('1', 0); //停止 Move('2', 0.39); xbee.printf("now_direction=%f\r\n", direction); break; } else { Move('4', 0.19);//時計回りに回転 xbee.printf("now_direction=%lf\r\n", direction); direction = AngleGet(next_CP_x,next_CP_y); } } catchGPS(); xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude); double lati = 111132.8715; //1度あたりの緯度の距離(m) double longi = 91535.79099; //1度あたりの経度の距離(m) GPS_x = gps.latitude; GPS_y = gps.longitude; if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x)*lati*lati + (next_CP_y - GPS_y)*(next_CP_y - GPS_y)*longi*longi < 12.25) { // CP到着判定 //試験で調整 xbee.printf("!!!!!!!!now leach cp[%d]=x_%f,y_%f!!!!!!!!!!!\r\n",i,next_CP_x ,next_CP_y); break; } speak(); }//while(1){} }//for(){} // 行動フロー終了 xbee.printf("End\r\n"); Move('2', 0.17); wait(0.2); Move('1', 0); //停止 return 0; }