CanSat-C 2021 / Mbed 2 deprecated cansatC_test2_xbee

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

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

main.cpp@5:cc7917e8c442, 2021-11-10 (annotated)

Committer:
miyajitakenari
Date:
Wed Nov 10 12:17:25 2021 +0000
Revision:
5:cc7917e8c442
Parent:
4:975b0d9bd51b
Child:
6:326208aabe68
a

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 //
miyajitakenari 0:79033ee3c961 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への向き
miyajitakenari 0:79033ee3c961 17 double CPs_x[3]={34.54608391847546, 34.545845666047306, 34.545666059919796}; //CPリスト(x座標)
miyajitakenari 0:79033ee3c961 18 double CPs_y[3]={135.50338843400897, 135.50368659080985, 135.50347298593758}; // 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 0:79033ee3c961 24 wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
miyajitakenari 3:ec2b7587be78 25 while(flight_pin){}
miyajitakenari 0:79033ee3c961 26 pc.printf("flight_pin nuketa");
miyajitakenari 0:79033ee3c961 27 xbee.printf("flight_pin nuketa");
miyajitakenari 0:79033ee3c961 28 wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
miyajitakenari 0:79033ee3c961 29 nichrome=1;
miyajitakenari 0:79033ee3c961 30 pc.printf("nichrome in");
miyajitakenari 0:79033ee3c961 31 xbee.printf("nichrome in");
miyajitakenari 0:79033ee3c961 32 wait(30);
miyajitakenari 0:79033ee3c961 33 nichrome=0;
miyajitakenari 0:79033ee3c961 34 // 落下終了
miyajitakenari 0:79033ee3c961 35
miyajitakenari 0:79033ee3c961 36
miyajitakenari 0:79033ee3c961 37 // 行動フロー開始
miyajitakenari 0:79033ee3c961 38 Calibration();
miyajitakenari 0:79033ee3c961 39 xbee.printf("XBee Connected\r\n");
miyajitakenari 0:79033ee3c961 40 pc.printf("xbee_Connected\r\n");
miyajitakenari 0:79033ee3c961 41 pc.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:79033ee3c961 42 xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:79033ee3c961 43 for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動
miyajitakenari 0:79033ee3c961 44 next_CP_x = CPs_x[i];
miyajitakenari 0:79033ee3c961 45 next_CP_y = CPs_y[i];
miyajitakenari 0:79033ee3c961 46
miyajitakenari 0:79033ee3c961 47 pc.printf("next_i=%d\r\n", i);
miyajitakenari 0:79033ee3c961 48 xbee.printf("next_i=%d\r\n", i);
miyajitakenari 0:79033ee3c961 49
miyajitakenari 0:79033ee3c961 50 while (1) {
miyajitakenari 0:79033ee3c961 51 speak();
miyajitakenari 0:79033ee3c961 52 direction = AngleGet();
miyajitakenari 5:cc7917e8c442 53 pc.printf("direction=%f\n\rdirection start", direction);
miyajitakenari 5:cc7917e8c442 54 xbee.printf("direction=%f\n\rdirection start", direction);
miyajitakenari 5:cc7917e8c442 55 int df=1;
miyajitakenari 0:79033ee3c961 56 //角度調節
miyajitakenari 0:79033ee3c961 57 while(1) {
miyajitakenari 0:79033ee3c961 58 if(direction < 5 || direction > 355) { //角度判定
miyajitakenari 5:cc7917e8c442 59 pc.printf("direction finish\n\r");
miyajitakenari 5:cc7917e8c442 60 xbee.printf("direction finish\n\r");
miyajitakenari 0:79033ee3c961 61 break;
miyajitakenari 0:79033ee3c961 62 }
miyajitakenari 0:79033ee3c961 63 else {
miyajitakenari 0:79033ee3c961 64 Move('1', 0);//停止
miyajitakenari 5:cc7917e8c442 65 if(df==1){
miyajitakenari 5:cc7917e8c442 66 pc.printf("mortor mode:1 speed:0\n\r");
miyajitakenari 5:cc7917e8c442 67 xbee.printf("mortor mode:1 speed:0\n\r");
miyajitakenari 5:cc7917e8c442 68 }
miyajitakenari 0:79033ee3c961 69 Move('4', 0.5);//時計回りに回転
miyajitakenari 5:cc7917e8c442 70 if(df==1){
miyajitakenari 5:cc7917e8c442 71 pc.printf("mortor mode:4 speed:0.5\n\r");
miyajitakenari 5:cc7917e8c442 72 pc.printf("mortor mode:4 speed:0.5\n\r");
miyajitakenari 5:cc7917e8c442 73 df++;
miyajitakenari 5:cc7917e8c442 74 direction = AngleGet();
miyajitakenari 5:cc7917e8c442 75 }
miyajitakenari 0:79033ee3c961 76 }
miyajitakenari 0:79033ee3c961 77 }
miyajitakenari 0:79033ee3c961 78 while(FrontGet()) {
miyajitakenari 5:cc7917e8c442 79 pc.printf("front get\n\r");
miyajitakenari 5:cc7917e8c442 80 xbee.printf("frontget\n\r");
miyajitakenari 0:79033ee3c961 81 Move('1', 0); //停止
miyajitakenari 5:cc7917e8c442 82 pc.printf("mortor mode:1 speed:0\n\r");
miyajitakenari 5:cc7917e8c442 83 xbee.printf("mortor mode:1 speed:0\n\r");
miyajitakenari 0:79033ee3c961 84 Move('4', 0.5); //時計回り回転
miyajitakenari 5:cc7917e8c442 85 pc.printf("mortor mode:4 speed:0.5\n\r");
miyajitakenari 5:cc7917e8c442 86 xbee.printf("mortor mode:4 speed:0.5\n\r");
miyajitakenari 0:79033ee3c961 87 wait(1);
miyajitakenari 0:79033ee3c961 88 Move('1', 0); //回転停止
miyajitakenari 5:cc7917e8c442 89 pc.printf("mortor mode:1 speed:0\n\r");
miyajitakenari 5:cc7917e8c442 90 xbee.printf("mortor mode:1 speed:0\n\r");
miyajitakenari 0:79033ee3c961 91 }
miyajitakenari 5:cc7917e8c442 92 pc.printf("flag=xbee ni Input");
miyajitakenari 5:cc7917e8c442 93 xbee.printf("speed flag=");
miyajitakenari 0:79033ee3c961 94 wait(3);
miyajitakenari 0:79033ee3c961 95 float as;//advance speed
miyajitakenari 0:79033ee3c961 96 if(xbee.readable()){
miyajitakenari 0:79033ee3c961 97 pc.printf("advance speed=xbee Input");
miyajitakenari 0:79033ee3c961 98 xbee.printf("advance speed=");
miyajitakenari 0:79033ee3c961 99 xbee.scanf("%f",&as);
miyajitakenari 0:79033ee3c961 100 }else{
miyajitakenari 0:79033ee3c961 101 as=0.5;
miyajitakenari 0:79033ee3c961 102 }
miyajitakenari 0:79033ee3c961 103 Move('2', as);
miyajitakenari 3:ec2b7587be78 104 pc.printf("mortor mode:2 speed:%f",as);
miyajitakenari 3:ec2b7587be78 105 xbee.printf("mortor mode:2 speed:%f",as);
miyajitakenari 0:79033ee3c961 106 catchGPS();
miyajitakenari 0:79033ee3c961 107 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:79033ee3c961 108 xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:79033ee3c961 109
miyajitakenari 0:79033ee3c961 110 double lati = 111132.8715; //1度あたりの緯度の距離(m)
miyajitakenari 0:79033ee3c961 111 double longi = 91535.79099; //1度あたりの経度の距離(m)
miyajitakenari 4:975b0d9bd51b 112 GPS_x = gps.latitude;
miyajitakenari 4:975b0d9bd51b 113 GPS_y = gps.longitude;
miyajitakenari 4:975b0d9bd51b 114 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:79033ee3c961 115 pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
miyajitakenari 0:79033ee3c961 116 xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
miyajitakenari 0:79033ee3c961 117 break;
miyajitakenari 0:79033ee3c961 118 }
miyajitakenari 0:79033ee3c961 119
miyajitakenari 0:79033ee3c961 120 }//while(1){}
miyajitakenari 0:79033ee3c961 121 }//for(){}
miyajitakenari 0:79033ee3c961 122 // 行動フロー終了
miyajitakenari 0:79033ee3c961 123 pc.printf("End\r\n");
miyajitakenari 0:79033ee3c961 124 xbee.printf("End\r\n");
miyajitakenari 0:79033ee3c961 125 Move('1', 0); //停止
miyajitakenari 3:ec2b7587be78 126 pc.printf("mortor mode:1 speed:0");
miyajitakenari 3:ec2b7587be78 127 xbee.printf("mortor mode:1 speed:0");
miyajitakenari 0:79033ee3c961 128 return 0;
miyajitakenari 0:79033ee3c961 129 }