speak割り込み 障害物の後、走ってから、目的地の方を向く
Dependencies: ATP3012 mbed a HMC US015_2 getGPS
main.cpp
- Committer:
- miyajitakenari
- Date:
- 2021-12-09
- Revision:
- 3:e137a2979ff1
- Parent:
- 2:72e09781a13d
- Child:
- 4:bd33b15ecac4
File content as of revision 3:e137a2979ff1:
/*ライブラリ*/ #include "mbed.h" // 自作関数 #include "Function.h" // フライトピン・ニクロム線関係 DigitalIn flight_pin(A0); DigitalOut nichrome(D13); #define cp_max 2 //CPの数を入力する //音声割り込み Ticker onsei_tick; void onsei(){ speak(); } int main() { // 変数宣言 double GPS_x, GPS_y; // 現在地の座標 double direction; // 次CPへの向き double CPs_x[2]={34.545588426585454, 34.5454484832847}; //CPリスト(x座標) double CPs_y[2]={135.50282053551706, 135.50262437335775}; // CPリスト(y座標) double next_CP_x, next_CP_y; // 落下検知 // パラシュート分離 wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん while(flight_pin){} xbee.printf("flight_pin nuketa"); wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s //nichrome=1; xbee.printf("nichrome in"); 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); onsei_tick.attach(&onsei, 10.0);//10秒ごとにavoid割り込み while (1) { while(FrontGet()){ xbee.printf("frontget\n\r"); Move('1', 0); //停止 Move('4', 0.2); //時計回り回転 wait(0.5); Move('1', 0); //回転停止 xbee.printf("front_avoid_rotate\n\r"); } //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも Move('2', 0.1); wait(1); Move('2', 0.5); wait(2); Move('1', 0); //ちょっと走るのおわり //走りながらanglegetできたら、止まらない //方向測定 direction = AngleGet(); xbee.printf("direction=%f\n\rrotation_start", direction); //角度調節 while(1) { if(direction < 20 || direction > 340) { xbee.printf("direction finish\n\r"); Move('1', 0); //停止 Move('2', 0.5); xbee.printf("now_angle=%f\r\n", direction); break; } else { Move('4', 0.15);//時計回りに回転 xbee.printf("now_angle=%lf\r\n", direction); } } xbee.printf("speed flag="); wait(3); float as[2];//advance speed if(xbee.readable()){ xbee.printf("advance speed="); xbee.scanf("%f",&as[1]); }else{ as[1]=0.5; } Move('2', as[1]); xbee.printf("mortor mode:2 speed:%f",as[1]); 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 < 25) { // CP到着判定 //試験で調整 xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y); break; } }//while(1){} }//for(){} // 行動フロー終了 xbee.printf("End\r\n"); Move('1', 0); //停止 xbee.printf("mortor mode:1 speed:0"); return 0; }