CanSat-C 2021 / Mbed 2 deprecated warikomi_flag1

flagを1つにした。frontgetを割り込みにした

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

main.cpp@2:5a54fa1f8e25, 2021-12-17 (annotated)

Committer:
miyajitakenari
Date:
Fri Dec 17 08:21:28 2021 +0000
Revision:
2:5a54fa1f8e25
Parent:
1:6d1adfccd142 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
miyajitakenari 0:18be66de24f7 1 /*ライブラリ*/
miyajitakenari 0:18be66de24f7 2 #include "mbed.h"
miyajitakenari 0:18be66de24f7 3
miyajitakenari 0:18be66de24f7 4 // 自作関数
miyajitakenari 0:18be66de24f7 5 #include "Function.h"
miyajitakenari 0:18be66de24f7 6
miyajitakenari 0:18be66de24f7 7 // フライトピン・ニクロム線関係
miyajitakenari 0:18be66de24f7 8 DigitalIn flight_pin(A0);
miyajitakenari 0:18be66de24f7 9 DigitalOut nichrome(D13);
miyajitakenari 0:18be66de24f7 10
miyajitakenari 0:18be66de24f7 11 #define cp_max 2 //CPの数を入力する
miyajitakenari 0:18be66de24f7 12
miyajitakenari 0:18be66de24f7 13 //音声割り込み
miyajitakenari 0:18be66de24f7 14 Ticker front_get;
miyajitakenari 0:18be66de24f7 15 void avoid(){
miyajitakenari 0:18be66de24f7 16 while(FrontGet()){
miyajitakenari 0:18be66de24f7 17 xbee.printf("frontget\n\r");
miyajitakenari 0:18be66de24f7 18 Move('1', 0); //停止
miyajitakenari 0:18be66de24f7 19 Move('4', 0.2); //時計回り回転
miyajitakenari 0:18be66de24f7 20 wait(0.5);
miyajitakenari 0:18be66de24f7 21 Move('1', 0); //回転停止
miyajitakenari 0:18be66de24f7 22 xbee.printf("front_avoid_rotate\n\r");
miyajitakenari 0:18be66de24f7 23 }
miyajitakenari 0:18be66de24f7 24 }
miyajitakenari 0:18be66de24f7 25
miyajitakenari 0:18be66de24f7 26 int main() {
miyajitakenari 0:18be66de24f7 27 // 変数宣言
miyajitakenari 0:18be66de24f7 28 double GPS_x, GPS_y; // 現在地の座標
miyajitakenari 0:18be66de24f7 29 double direction; // 次CPへの向き
miyajitakenari 0:18be66de24f7 30 double CPs_x[2]={34.545588426585454, 34.5454484832847}; //CPリスト(x座標)
miyajitakenari 0:18be66de24f7 31 double CPs_y[2]={135.50282053551706, 135.50262437335775}; // CPリスト(y座標)
miyajitakenari 0:18be66de24f7 32 double next_CP_x, next_CP_y;
miyajitakenari 0:18be66de24f7 33
miyajitakenari 0:18be66de24f7 34 // 落下検知
miyajitakenari 0:18be66de24f7 35 // パラシュート分離
miyajitakenari 0:18be66de24f7 36
miyajitakenari 0:18be66de24f7 37 wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
miyajitakenari 0:18be66de24f7 38 while(flight_pin){}
miyajitakenari 0:18be66de24f7 39 xbee.printf("flight_pin nuketa");
miyajitakenari 0:18be66de24f7 40 wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s//本番戻す
miyajitakenari 0:18be66de24f7 41 //nichrome=1;
miyajitakenari 0:18be66de24f7 42 xbee.printf("nichrome in");
miyajitakenari 0:18be66de24f7 43 wait(10);
miyajitakenari 0:18be66de24f7 44 nichrome=0;
miyajitakenari 0:18be66de24f7 45 // 落下終了
miyajitakenari 0:18be66de24f7 46
miyajitakenari 0:18be66de24f7 47
miyajitakenari 0:18be66de24f7 48 // 行動フロー開始
miyajitakenari 0:18be66de24f7 49 Calibration();
miyajitakenari 0:18be66de24f7 50 xbee.printf("XBee Connected\r\n");
miyajitakenari 0:18be66de24f7 51 xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:18be66de24f7 52 for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動
miyajitakenari 0:18be66de24f7 53 next_CP_x = CPs_x[i];
miyajitakenari 0:18be66de24f7 54 next_CP_y = CPs_y[i];
miyajitakenari 0:18be66de24f7 55
miyajitakenari 0:18be66de24f7 56 xbee.printf("next_i=%d\r\n", i);
miyajitakenari 0:18be66de24f7 57 front_get.attach(&avoid, 10.0);//10秒ごとにavoid割り込み
miyajitakenari 0:18be66de24f7 58
miyajitakenari 0:18be66de24f7 59 while (1) {
miyajitakenari 0:18be66de24f7 60 //方向測定
miyajitakenari 0:18be66de24f7 61 direction = AngleGet();
miyajitakenari 0:18be66de24f7 62 xbee.printf("direction=%f\n\rrotation_start", direction);
miyajitakenari 0:18be66de24f7 63 //角度調節
miyajitakenari 0:18be66de24f7 64 while(1) {
miyajitakenari 0:18be66de24f7 65 if(direction < 20 || direction > 340) {
miyajitakenari 0:18be66de24f7 66 xbee.printf("direction finish\n\r");
miyajitakenari 0:18be66de24f7 67 Move('1', 0); //停止
miyajitakenari 0:18be66de24f7 68 Move('2', 0.5);
miyajitakenari 0:18be66de24f7 69 xbee.printf("now_angle=%f\r\n", direction);
miyajitakenari 0:18be66de24f7 70 break;
miyajitakenari 0:18be66de24f7 71 }
miyajitakenari 0:18be66de24f7 72 else {
miyajitakenari 0:18be66de24f7 73 Move('4', 0.15);//時計回りに回転
miyajitakenari 0:18be66de24f7 74 xbee.printf("now_angle=%lf\r\n", direction);
miyajitakenari 0:18be66de24f7 75 direction = AngleGet();
miyajitakenari 0:18be66de24f7 76 }
miyajitakenari 0:18be66de24f7 77 }
miyajitakenari 1:6d1adfccd142 78 speak();
miyajitakenari 0:18be66de24f7 79 catchGPS();
miyajitakenari 0:18be66de24f7 80 xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:18be66de24f7 81
miyajitakenari 0:18be66de24f7 82 double lati = 111132.8715; //1度あたりの緯度の距離(m)
miyajitakenari 0:18be66de24f7 83 double longi = 91535.79099; //1度あたりの経度の距離(m)
miyajitakenari 0:18be66de24f7 84 GPS_x = gps.latitude;
miyajitakenari 0:18be66de24f7 85 GPS_y = gps.longitude;
miyajitakenari 0:18be66de24f7 86 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:18be66de24f7 87 xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
miyajitakenari 0:18be66de24f7 88 break;
miyajitakenari 0:18be66de24f7 89 }
miyajitakenari 0:18be66de24f7 90
miyajitakenari 0:18be66de24f7 91 }//while(1){}
miyajitakenari 0:18be66de24f7 92 }//for(){}
miyajitakenari 0:18be66de24f7 93 // 行動フロー終了
miyajitakenari 0:18be66de24f7 94 xbee.printf("End\r\n");
miyajitakenari 0:18be66de24f7 95 Move('1', 0); //停止
miyajitakenari 0:18be66de24f7 96 xbee.printf("mortor mode:1 speed:0");
miyajitakenari 0:18be66de24f7 97 return 0;
miyajitakenari 0:18be66de24f7 98 }