speak割り込み 障害物の後、走ってから、目的地の方を向く
Dependencies: ATP3012 mbed a HMC US015_2 getGPS
main.cpp@0:dbc31c76716d, 2021-12-06 (annotated)
- Committer:
- miyajitakenari
- Date:
- Mon Dec 06 09:53:05 2021 +0000
- Revision:
- 0:dbc31c76716d
- Child:
- 1:7e0593a7b534
speak warikomi; avoid no ato hasittekara direction rotate
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
miyajitakenari | 0:dbc31c76716d | 1 | /*ライブラリ*/ |
miyajitakenari | 0:dbc31c76716d | 2 | #include "mbed.h" |
miyajitakenari | 0:dbc31c76716d | 3 | |
miyajitakenari | 0:dbc31c76716d | 4 | // 自作関数 |
miyajitakenari | 0:dbc31c76716d | 5 | #include "Function.h" |
miyajitakenari | 0:dbc31c76716d | 6 | |
miyajitakenari | 0:dbc31c76716d | 7 | // フライトピン・ニクロム線関係 |
miyajitakenari | 0:dbc31c76716d | 8 | DigitalIn flight_pin(A0); |
miyajitakenari | 0:dbc31c76716d | 9 | DigitalOut nichrome(D13); |
miyajitakenari | 0:dbc31c76716d | 10 | |
miyajitakenari | 0:dbc31c76716d | 11 | #define cp_max 3 //CPの数を入力する |
miyajitakenari | 0:dbc31c76716d | 12 | |
miyajitakenari | 0:dbc31c76716d | 13 | //音声割り込み |
miyajitakenari | 0:dbc31c76716d | 14 | Ticker onsei_tick; |
miyajitakenari | 0:dbc31c76716d | 15 | void onsei(){ |
miyajitakenari | 0:dbc31c76716d | 16 | speak(); |
miyajitakenari | 0:dbc31c76716d | 17 | } |
miyajitakenari | 0:dbc31c76716d | 18 | |
miyajitakenari | 0:dbc31c76716d | 19 | int main() { |
miyajitakenari | 0:dbc31c76716d | 20 | // 変数宣言 |
miyajitakenari | 0:dbc31c76716d | 21 | double GPS_x, GPS_y; // 現在地の座標 |
miyajitakenari | 0:dbc31c76716d | 22 | double direction; // 次CPへの向き |
miyajitakenari | 0:dbc31c76716d | 23 | double CPs_x[3]={34.54608391847546, 34.545845666047306, 34.545666059919796}; //CPリスト(x座標) |
miyajitakenari | 0:dbc31c76716d | 24 | double CPs_y[3]={135.50338843400897, 135.50368659080985, 135.50347298593758}; // CPリスト(y座標) |
miyajitakenari | 0:dbc31c76716d | 25 | double next_CP_x, next_CP_y; |
miyajitakenari | 0:dbc31c76716d | 26 | |
miyajitakenari | 0:dbc31c76716d | 27 | // 落下検知 |
miyajitakenari | 0:dbc31c76716d | 28 | // パラシュート分離 |
miyajitakenari | 0:dbc31c76716d | 29 | |
miyajitakenari | 0:dbc31c76716d | 30 | /*wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん |
miyajitakenari | 0:dbc31c76716d | 31 | while(flight_pin){} |
miyajitakenari | 0:dbc31c76716d | 32 | xbee.printf("flight_pin nuketa"); |
miyajitakenari | 0:dbc31c76716d | 33 | wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s |
miyajitakenari | 0:dbc31c76716d | 34 | nichrome=1; |
miyajitakenari | 0:dbc31c76716d | 35 | xbee.printf("nichrome in"); |
miyajitakenari | 0:dbc31c76716d | 36 | wait(30); |
miyajitakenari | 0:dbc31c76716d | 37 | nichrome=0;*/ |
miyajitakenari | 0:dbc31c76716d | 38 | // 落下終了 |
miyajitakenari | 0:dbc31c76716d | 39 | |
miyajitakenari | 0:dbc31c76716d | 40 | |
miyajitakenari | 0:dbc31c76716d | 41 | // 行動フロー開始 |
miyajitakenari | 0:dbc31c76716d | 42 | Calibration(); |
miyajitakenari | 0:dbc31c76716d | 43 | xbee.printf("XBee Connected\r\n"); |
miyajitakenari | 0:dbc31c76716d | 44 | xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude); |
miyajitakenari | 0:dbc31c76716d | 45 | for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動 |
miyajitakenari | 0:dbc31c76716d | 46 | next_CP_x = CPs_x[i]; |
miyajitakenari | 0:dbc31c76716d | 47 | next_CP_y = CPs_y[i]; |
miyajitakenari | 0:dbc31c76716d | 48 | |
miyajitakenari | 0:dbc31c76716d | 49 | xbee.printf("next_i=%d\r\n", i); |
miyajitakenari | 0:dbc31c76716d | 50 | onsei_tick.attach(&onsei, 10.0);//10秒ごとにavoid割り込み |
miyajitakenari | 0:dbc31c76716d | 51 | |
miyajitakenari | 0:dbc31c76716d | 52 | while (1) { |
miyajitakenari | 0:dbc31c76716d | 53 | while(FrontGet()){ |
miyajitakenari | 0:dbc31c76716d | 54 | xbee.printf("frontget\n\r"); |
miyajitakenari | 0:dbc31c76716d | 55 | Move('1', 0); //停止 |
miyajitakenari | 0:dbc31c76716d | 56 | Move('4', 0.5); //時計回り回転 |
miyajitakenari | 0:dbc31c76716d | 57 | wait(1); |
miyajitakenari | 0:dbc31c76716d | 58 | Move('1', 0); //回転停止 |
miyajitakenari | 0:dbc31c76716d | 59 | xbee.printf("front_avoid_rotate\n\r"); |
miyajitakenari | 0:dbc31c76716d | 60 | } |
miyajitakenari | 0:dbc31c76716d | 61 | //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも |
miyajitakenari | 0:dbc31c76716d | 62 | Move('2', 0.1); |
miyajitakenari | 0:dbc31c76716d | 63 | wait(1); |
miyajitakenari | 0:dbc31c76716d | 64 | Move('2', 0.5); |
miyajitakenari | 0:dbc31c76716d | 65 | wait(2); |
miyajitakenari | 0:dbc31c76716d | 66 | Move('1', 0); |
miyajitakenari | 0:dbc31c76716d | 67 | //ちょっと走るのおわり |
miyajitakenari | 0:dbc31c76716d | 68 | |
miyajitakenari | 0:dbc31c76716d | 69 | //方向測定 |
miyajitakenari | 0:dbc31c76716d | 70 | direction = AngleGet(); |
miyajitakenari | 0:dbc31c76716d | 71 | xbee.printf("direction=%f\n\rrotation_start", direction); |
miyajitakenari | 0:dbc31c76716d | 72 | //角度調節 |
miyajitakenari | 0:dbc31c76716d | 73 | while(1) { |
miyajitakenari | 0:dbc31c76716d | 74 | if(direction < 20 || direction > 340) { |
miyajitakenari | 0:dbc31c76716d | 75 | xbee.printf("direction finish\n\r"); |
miyajitakenari | 0:dbc31c76716d | 76 | Move('1', 0); //停止 |
miyajitakenari | 0:dbc31c76716d | 77 | wait(5); |
miyajitakenari | 0:dbc31c76716d | 78 | xbee.printf("now_angle=%f\r\n", direction); |
miyajitakenari | 0:dbc31c76716d | 79 | break; |
miyajitakenari | 0:dbc31c76716d | 80 | } |
miyajitakenari | 0:dbc31c76716d | 81 | else { |
miyajitakenari | 0:dbc31c76716d | 82 | Move('4', 0.15);//時計回りに回転 |
miyajitakenari | 0:dbc31c76716d | 83 | xbee.printf("now_angle=%lf\r\n", direction); |
miyajitakenari | 0:dbc31c76716d | 84 | } |
miyajitakenari | 0:dbc31c76716d | 85 | } |
miyajitakenari | 0:dbc31c76716d | 86 | |
miyajitakenari | 0:dbc31c76716d | 87 | xbee.printf("speed flag="); |
miyajitakenari | 0:dbc31c76716d | 88 | wait(3); |
miyajitakenari | 0:dbc31c76716d | 89 | float as[2];//advance speed |
miyajitakenari | 0:dbc31c76716d | 90 | if(xbee.readable()){ |
miyajitakenari | 0:dbc31c76716d | 91 | xbee.printf("advance speed="); |
miyajitakenari | 0:dbc31c76716d | 92 | xbee.scanf("%f",&as[1]); |
miyajitakenari | 0:dbc31c76716d | 93 | }else{ |
miyajitakenari | 0:dbc31c76716d | 94 | as[1]=0.5; |
miyajitakenari | 0:dbc31c76716d | 95 | } |
miyajitakenari | 0:dbc31c76716d | 96 | Move('2', as[1]); |
miyajitakenari | 0:dbc31c76716d | 97 | xbee.printf("mortor mode:2 speed:%f",as[1]); |
miyajitakenari | 0:dbc31c76716d | 98 | catchGPS(); |
miyajitakenari | 0:dbc31c76716d | 99 | xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude); |
miyajitakenari | 0:dbc31c76716d | 100 | |
miyajitakenari | 0:dbc31c76716d | 101 | double lati = 111132.8715; //1度あたりの緯度の距離(m) |
miyajitakenari | 0:dbc31c76716d | 102 | double longi = 91535.79099; //1度あたりの経度の距離(m) |
miyajitakenari | 0:dbc31c76716d | 103 | GPS_x = gps.latitude; |
miyajitakenari | 0:dbc31c76716d | 104 | GPS_y = gps.longitude; |
miyajitakenari | 0:dbc31c76716d | 105 | 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到着判定 //試験で調整 |
miyajitakenari | 0:dbc31c76716d | 106 | xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y); |
miyajitakenari | 0:dbc31c76716d | 107 | break; |
miyajitakenari | 0:dbc31c76716d | 108 | } |
miyajitakenari | 0:dbc31c76716d | 109 | |
miyajitakenari | 0:dbc31c76716d | 110 | }//while(1){} |
miyajitakenari | 0:dbc31c76716d | 111 | }//for(){} |
miyajitakenari | 0:dbc31c76716d | 112 | // 行動フロー終了 |
miyajitakenari | 0:dbc31c76716d | 113 | xbee.printf("End\r\n"); |
miyajitakenari | 0:dbc31c76716d | 114 | Move('1', 0); //停止 |
miyajitakenari | 0:dbc31c76716d | 115 | xbee.printf("mortor mode:1 speed:0"); |
miyajitakenari | 0:dbc31c76716d | 116 | return 0; |
miyajitakenari | 0:dbc31c76716d | 117 | } |