CanSat-C 2021 / Mbed 2 deprecated cansatC_test2_warikomi

speak割り込み 障害物の後、走ってから、目的地の方を向く

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

main.cpp@4:bd33b15ecac4, 2021-12-13 (annotated)

Committer:
miyajitakenari
Date:
Mon Dec 13 10:07:09 2021 +0000
Revision:
4:bd33b15ecac4
Parent:
3:e137a2979ff1 
a

Who changed what in which revision?

UserRevisionLine numberNew 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 1:7e0593a7b534 11 #define cp_max 2 //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 1:7e0593a7b534 23 double CPs_x[2]={34.545588426585454, 34.5454484832847}; //CPリスト(x座標)
miyajitakenari 1:7e0593a7b534 24 double CPs_y[2]={135.50282053551706, 135.50262437335775}; // 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 1:7e0593a7b534 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 1:7e0593a7b534 34 //nichrome=1;
miyajitakenari 0:dbc31c76716d 35 xbee.printf("nichrome in");
miyajitakenari 1:7e0593a7b534 36 wait(10);
miyajitakenari 1:7e0593a7b534 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 2:72e09781a13d 56 Move('4', 0.2); //時計回り回転
miyajitakenari 2:72e09781a13d 57 wait(0.5);
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 3:e137a2979ff1 68 //走りながらanglegetできたら、止まらない
miyajitakenari 0:dbc31c76716d 69
miyajitakenari 0:dbc31c76716d 70 //方向測定
miyajitakenari 0:dbc31c76716d 71 direction = AngleGet();
miyajitakenari 0:dbc31c76716d 72 xbee.printf("direction=%f\n\rrotation_start", direction);
miyajitakenari 0:dbc31c76716d 73 //角度調節
miyajitakenari 0:dbc31c76716d 74 while(1) {
miyajitakenari 0:dbc31c76716d 75 if(direction < 20 || direction > 340) {
miyajitakenari 0:dbc31c76716d 76 xbee.printf("direction finish\n\r");
miyajitakenari 0:dbc31c76716d 77 Move('1', 0); //停止
miyajitakenari 2:72e09781a13d 78 Move('2', 0.5);
miyajitakenari 0:dbc31c76716d 79 xbee.printf("now_angle=%f\r\n", direction);
miyajitakenari 0:dbc31c76716d 80 break;
miyajitakenari 0:dbc31c76716d 81 }
miyajitakenari 0:dbc31c76716d 82 else {
miyajitakenari 0:dbc31c76716d 83 Move('4', 0.15);//時計回りに回転
miyajitakenari 0:dbc31c76716d 84 xbee.printf("now_angle=%lf\r\n", direction);
miyajitakenari 4:bd33b15ecac4 85 direction = AngleGet();
miyajitakenari 0:dbc31c76716d 86 }
miyajitakenari 0:dbc31c76716d 87 }
miyajitakenari 0:dbc31c76716d 88
miyajitakenari 0:dbc31c76716d 89 xbee.printf("speed flag=");
miyajitakenari 0:dbc31c76716d 90 wait(3);
miyajitakenari 0:dbc31c76716d 91 float as[2];//advance speed
miyajitakenari 0:dbc31c76716d 92 if(xbee.readable()){
miyajitakenari 0:dbc31c76716d 93 xbee.printf("advance speed=");
miyajitakenari 0:dbc31c76716d 94 xbee.scanf("%f",&as[1]);
miyajitakenari 0:dbc31c76716d 95 }else{
miyajitakenari 0:dbc31c76716d 96 as[1]=0.5;
miyajitakenari 0:dbc31c76716d 97 }
miyajitakenari 0:dbc31c76716d 98 Move('2', as[1]);
miyajitakenari 0:dbc31c76716d 99 xbee.printf("mortor mode:2 speed:%f",as[1]);
miyajitakenari 0:dbc31c76716d 100 catchGPS();
miyajitakenari 0:dbc31c76716d 101 xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:dbc31c76716d 102
miyajitakenari 0:dbc31c76716d 103 double lati = 111132.8715; //1度あたりの緯度の距離(m)
miyajitakenari 0:dbc31c76716d 104 double longi = 91535.79099; //1度あたりの経度の距離(m)
miyajitakenari 0:dbc31c76716d 105 GPS_x = gps.latitude;
miyajitakenari 0:dbc31c76716d 106 GPS_y = gps.longitude;
miyajitakenari 0:dbc31c76716d 107 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 108 xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
miyajitakenari 0:dbc31c76716d 109 break;
miyajitakenari 0:dbc31c76716d 110 }
miyajitakenari 0:dbc31c76716d 111
miyajitakenari 0:dbc31c76716d 112 }//while(1){}
miyajitakenari 0:dbc31c76716d 113 }//for(){}
miyajitakenari 0:dbc31c76716d 114 // 行動フロー終了
miyajitakenari 0:dbc31c76716d 115 xbee.printf("End\r\n");
miyajitakenari 0:dbc31c76716d 116 Move('1', 0); //停止
miyajitakenari 0:dbc31c76716d 117 xbee.printf("mortor mode:1 speed:0");
miyajitakenari 0:dbc31c76716d 118 return 0;
miyajitakenari 0:dbc31c76716d 119 }