CanSat-C 2021 / Mbed 2 deprecated test_notchange

気球試験1,2回目 3回目はtest2-xbee

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

main.cpp@0:e386cbba36d5, 2021-12-19 (annotated)

Committer:
miyajitakenari
Date:
Sun Dec 19 09:31:36 2021 +0000
Revision:
0:e386cbba36d5
1,2kaime

Who changed what in which revision?

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