CanSat-C 2021 / Mbed 2 deprecated cansatC_test2_xbee

Xbeeを実装、speakをfunctionに、rotate消した、calibrationで回るように、ゴール判定を消した

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

main.cpp@23:6722c5f0c3f3, 2021-12-17 (annotated)

Committer:
gakutoagata
Date:
Fri Dec 17 07:24:51 2021 +0000
Revision:
23:6722c5f0c3f3
Parent:
22:23d9592bca04
Child:
24:b9b252fbee8e
12/17

Who changed what in which revision?

UserRevisionLine numberNew contents of line
miyajitakenari 2:9bbc22250488 1 /*ライブラリ*/
miyajitakenari 0:79033ee3c961 2 #include "mbed.h"
miyajitakenari 0:79033ee3c961 3
miyajitakenari 0:79033ee3c961 4 // 自作関数
miyajitakenari 0:79033ee3c961 5 #include "Function.h"
miyajitakenari 0:79033ee3c961 6
miyajitakenari 0:79033ee3c961 7 // フライトピン・ニクロム線関係
miyajitakenari 0:79033ee3c961 8 DigitalIn flight_pin(A0);
miyajitakenari 0:79033ee3c961 9 DigitalOut nichrome(D13);
miyajitakenari 0:79033ee3c961 10 //
gakutoagata 23:6722c5f0c3f3 11 #define cp_max 3 //CPの数を入力する
miyajitakenari 0:79033ee3c961 12
miyajitakenari 0:79033ee3c961 13 int main() {
miyajitakenari 0:79033ee3c961 14 // 変数宣言
miyajitakenari 0:79033ee3c961 15 double GPS_x, GPS_y; // 現在地の座標
miyajitakenari 0:79033ee3c961 16 double direction; // 次CPへの向き
gakutoagata 23:6722c5f0c3f3 17 double CPs_x[3]={34.54475005065638,34.54501545998881,34.544879982295605, }; //CPリスト(x座標)
gakutoagata 23:6722c5f0c3f3 18 double CPs_y[3]={135.50554516096807,135.5058945348109, 135.50572453098422}; // CPリスト(y座標)
miyajitakenari 0:79033ee3c961 19 double next_CP_x, next_CP_y;
miyajitakenari 0:79033ee3c961 20
miyajitakenari 0:79033ee3c961 21 // 落下検知
miyajitakenari 0:79033ee3c961 22 // パラシュート分離
miyajitakenari 0:79033ee3c961 23
miyajitakenari 10:e25e06011fd2 24 wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
miyajitakenari 3:ec2b7587be78 25 while(flight_pin){}
miyajitakenari 16:42f2aa7edc2b 26 xbee.printf("flight_pin nuketa\n\r");
miyajitakenari 15:7ff8e5bbae88 27 wait(5);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
miyajitakenari 10:e25e06011fd2 28 //nichrome=1;
miyajitakenari 16:42f2aa7edc2b 29 xbee.printf("nichrome in\n\r");
miyajitakenari 9:c96ceecb43e1 30 wait(10);
miyajitakenari 10:e25e06011fd2 31 nichrome=0;
miyajitakenari 0:79033ee3c961 32 // 落下終了
miyajitakenari 0:79033ee3c961 33
miyajitakenari 0:79033ee3c961 34
miyajitakenari 0:79033ee3c961 35 // 行動フロー開始
miyajitakenari 0:79033ee3c961 36 Calibration();
miyajitakenari 0:79033ee3c961 37 xbee.printf("XBee Connected\r\n");
miyajitakenari 0:79033ee3c961 38 xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:79033ee3c961 39 for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動
miyajitakenari 0:79033ee3c961 40 next_CP_x = CPs_x[i];
miyajitakenari 0:79033ee3c961 41 next_CP_y = CPs_y[i];
miyajitakenari 0:79033ee3c961 42
miyajitakenari 0:79033ee3c961 43 xbee.printf("next_i=%d\r\n", i);
miyajitakenari 0:79033ee3c961 44
miyajitakenari 0:79033ee3c961 45 while (1) {
miyajitakenari 10:e25e06011fd2 46 speak();
miyajitakenari 6:326208aabe68 47 while(FrontGet()) {
miyajitakenari 6:326208aabe68 48 xbee.printf("frontget\n\r");
miyajitakenari 17:a1e6729a385e 49 Move('2', 0.1);
miyajitakenari 17:a1e6729a385e 50 wait(0.5);
miyajitakenari 6:326208aabe68 51 Move('1', 0); //停止
miyajitakenari 11:d00bfdcb7481 52 Move('4', 0.2); //時計回り回転
miyajitakenari 11:d00bfdcb7481 53 wait(0.5);
miyajitakenari 16:42f2aa7edc2b 54 /*Move('2', 0.17);
miyajitakenari 16:42f2aa7edc2b 55 wait(0.2);*/
miyajitakenari 6:326208aabe68 56 Move('1', 0); //回転停止
miyajitakenari 6:326208aabe68 57 xbee.printf("front_avoid_rotate\n\r");
miyajitakenari 6:326208aabe68 58 }
miyajitakenari 10:e25e06011fd2 59
miyajitakenari 10:e25e06011fd2 60 //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも
miyajitakenari 10:e25e06011fd2 61 Move('2', 0.1);
miyajitakenari 17:a1e6729a385e 62 wait(0.5);
miyajitakenari 15:7ff8e5bbae88 63 Move('2', 0.2);
miyajitakenari 17:a1e6729a385e 64 wait(1);
miyajitakenari 18:da59a86eb246 65 Move('2', 0.15);
miyajitakenari 15:7ff8e5bbae88 66 wait(0.2);
miyajitakenari 10:e25e06011fd2 67 Move('1', 0);
miyajitakenari 10:e25e06011fd2 68 //ちょっと走るのおわり
miyajitakenari 12:740d999b1fea 69 //走りながらanglegetできたら、止まらない
miyajitakenari 10:e25e06011fd2 70
miyajitakenari 19:6c56feca386d 71 catchGPS();
miyajitakenari 19:6c56feca386d 72 direction = AngleGet(next_CP_x,next_CP_y);
gakutoagata 23:6722c5f0c3f3 73 xbee.printf("\n\n\r----direction start-----\r\n");
gakutoagata 23:6722c5f0c3f3 74 xbee.printf("next_cp_x=%lf, next_cp_y=%lf\r\n",next_CP_x,next_CP_y);
miyajitakenari 0:79033ee3c961 75 //角度調節
miyajitakenari 0:79033ee3c961 76 while(1) {
gakutoagata 23:6722c5f0c3f3 77 if(direction < 5 || direction > 355) {
gakutoagata 23:6722c5f0c3f3 78 xbee.printf("\n-----direction finish-----\r\n");
miyajitakenari 6:326208aabe68 79 Move('1', 0); //停止
miyajitakenari 17:a1e6729a385e 80 Move('2', 0.39);
gakutoagata 23:6722c5f0c3f3 81 xbee.printf("now_direction=%f\r\n", direction);
miyajitakenari 0:79033ee3c961 82 break;
miyajitakenari 5:cc7917e8c442 83 }
miyajitakenari 6:326208aabe68 84 else {
miyajitakenari 18:da59a86eb246 85 Move('4', 0.19);//時計回りに回転
gakutoagata 23:6722c5f0c3f3 86 xbee.printf("now_direction=%lf\r\n", direction);
miyajitakenari 19:6c56feca386d 87 direction = AngleGet(next_CP_x,next_CP_y);
miyajitakenari 6:326208aabe68 88 }
miyajitakenari 0:79033ee3c961 89 }
miyajitakenari 6:326208aabe68 90
miyajitakenari 5:cc7917e8c442 91 xbee.printf("speed flag=");
miyajitakenari 0:79033ee3c961 92 wait(3);
miyajitakenari 6:326208aabe68 93 float as[2];//advance speed
miyajitakenari 0:79033ee3c961 94 if(xbee.readable()){
miyajitakenari 0:79033ee3c961 95 xbee.printf("advance speed=");
miyajitakenari 6:326208aabe68 96 xbee.scanf("%f",&as[1]);
miyajitakenari 0:79033ee3c961 97 }else{
miyajitakenari 17:a1e6729a385e 98 as[1]=0.39;
miyajitakenari 0:79033ee3c961 99 }
miyajitakenari 6:326208aabe68 100 Move('2', as[1]);
miyajitakenari 6:326208aabe68 101 xbee.printf("mortor mode:2 speed:%f",as[1]);
miyajitakenari 0:79033ee3c961 102 catchGPS();
miyajitakenari 0:79033ee3c961 103 xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:79033ee3c961 104
miyajitakenari 0:79033ee3c961 105 double lati = 111132.8715; //1度あたりの緯度の距離(m)
miyajitakenari 0:79033ee3c961 106 double longi = 91535.79099; //1度あたりの経度の距離(m)
miyajitakenari 4:975b0d9bd51b 107 GPS_x = gps.latitude;
miyajitakenari 4:975b0d9bd51b 108 GPS_y = gps.longitude;
miyajitakenari 4:975b0d9bd51b 109 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到着判定 //試験で調整
gakutoagata 23:6722c5f0c3f3 110 xbee.printf("!!!!!!!!now leach cp[%d]=x_%f,y_%f!!!!!!!!!!!\r\n",i,next_CP_x ,next_CP_y);
miyajitakenari 0:79033ee3c961 111 break;
miyajitakenari 0:79033ee3c961 112 }
miyajitakenari 0:79033ee3c961 113
miyajitakenari 0:79033ee3c961 114 }//while(1){}
miyajitakenari 0:79033ee3c961 115 }//for(){}
miyajitakenari 0:79033ee3c961 116 // 行動フロー終了
miyajitakenari 0:79033ee3c961 117 xbee.printf("End\r\n");
miyajitakenari 15:7ff8e5bbae88 118 Move('2', 0.17);
miyajitakenari 15:7ff8e5bbae88 119 wait(0.2);
miyajitakenari 0:79033ee3c961 120 Move('1', 0); //停止
miyajitakenari 0:79033ee3c961 121 return 0;
miyajitakenari 0:79033ee3c961 122 }