CanSat-C 2021 / Mbed 2 deprecated cansatC_test2_xbee2

12/16用テスト

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

main.cpp@21:52f8f01a29c5, 2021-12-16 (annotated)

Committer:
ushiroji
Date:
Thu Dec 16 06:10:06 2021 +0000
Revision:
21:52f8f01a29c5
Parent:
18:bd0b2394fa48 
add catchGPS to AngleGet; change direction < 10 || direction > 350

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 //
ushiroji 18:bd0b2394fa48 11 #define cp_max 5 //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への向き
ushiroji 18:bd0b2394fa48 17 double CPs_x[5]={34.54556786858123, 34.545733563643864, 34.54594565284262, 34.54614448597558, 34.54632012151458}; //CPリスト(x座標)
ushiroji 18:bd0b2394fa48 18 double CPs_y[5]={135.51009581235357, 135.51000193504115, 135.5098705068037, 135.50974444298416, 135.50962106137328}; // CPリスト(y座標)
miyajitakenari 0:79033ee3c961 19 double next_CP_x, next_CP_y;
ushiroji 18:bd0b2394fa48 20 //B6棟裏セット 34.545588426585454, 34.5454484832847 | 135.50282053551706, 135.50262437335775
ushiroji 18:bd0b2394fa48 21 //グラウンド道路近くの直線 34.54556786858123, 34.545733563643864, 34.54594565284262, 34.54614448597558, 34.54632012151458
ushiroji 18:bd0b2394fa48 22 // 135.51009581235357, 135.51000193504115, 135.5098705068037, 135.50974444298416, 135.50962106137328
miyajitakenari 0:79033ee3c961 23
ushiroji 14:3e7d563538e5 24 //落下開始
miyajitakenari 3:ec2b7587be78 25 while(flight_pin){}
ushiroji 14:3e7d563538e5 26 xbee.printf("flight_pin nuketa\n"); // 落下検知
ushiroji 14:3e7d563538e5 27 wait(5);//!ホントは35! //ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
ushiroji 14:3e7d563538e5 28 //nichrome=1; // パラシュート分離
miyajitakenari 13:65ae2ea75c44 29 xbee.printf("nichrome in\n");
miyajitakenari 9:c96ceecb43e1 30 wait(10);
miyajitakenari 10:e25e06011fd2 31 nichrome=0;
ushiroji 14:3e7d563538e5 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);
ushiroji 14:3e7d563538e5 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) {
ushiroji 14:3e7d563538e5 46 speak();
miyajitakenari 10:e25e06011fd2 47
ushiroji 14:3e7d563538e5 48 //回避開始
ushiroji 14:3e7d563538e5 49 while(FrontGet()) {
ushiroji 14:3e7d563538e5 50 xbee.printf("frontget\r\n");
ushiroji 14:3e7d563538e5 51 Move('1', 0); //停止
ushiroji 18:bd0b2394fa48 52 Move('4', 0.18); //時計回り回転
ushiroji 14:3e7d563538e5 53 wait(0.5);
ushiroji 14:3e7d563538e5 54 Move('1', 0); //回転停止
ushiroji 14:3e7d563538e5 55 xbee.printf("front_avoid_rotate\r\n");
ushiroji 14:3e7d563538e5 56 }
miyajitakenari 10:e25e06011fd2 57 //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも
miyajitakenari 10:e25e06011fd2 58 Move('2', 0.1);
miyajitakenari 10:e25e06011fd2 59 wait(1);
ushiroji 18:bd0b2394fa48 60 Move('2', 0.39);
miyajitakenari 10:e25e06011fd2 61 wait(2);
ushiroji 14:3e7d563538e5 62 Move('1', 0); //走りながらanglegetできたら、止まらない
miyajitakenari 10:e25e06011fd2 63 //ちょっと走るのおわり
ushiroji 14:3e7d563538e5 64 //回避終了
miyajitakenari 10:e25e06011fd2 65
ushiroji 14:3e7d563538e5 66 //角度調節開始
miyajitakenari 0:79033ee3c961 67 direction = AngleGet();
ushiroji 14:3e7d563538e5 68 xbee.printf("direction=%f\r\nrotation_start", direction);
miyajitakenari 0:79033ee3c961 69 while(1) {
ushiroji 21:52f8f01a29c5 70 if(direction < 10 || direction > 350) {
ushiroji 14:3e7d563538e5 71 xbee.printf("rotation finish\r\n");
miyajitakenari 6:326208aabe68 72 Move('1', 0); //停止
ushiroji 18:bd0b2394fa48 73 Move('2', 0.39);
miyajitakenari 6:326208aabe68 74 xbee.printf("now_angle=%f\r\n", direction);
miyajitakenari 0:79033ee3c961 75 break;
miyajitakenari 5:cc7917e8c442 76 }
miyajitakenari 6:326208aabe68 77 else {
ushiroji 18:bd0b2394fa48 78 Move('4', 0.18);//時計回りに回転
miyajitakenari 8:0f7e6ba9a434 79 xbee.printf("now_angle=%lf\r\n", direction);
ushiroji 14:3e7d563538e5 80 direction = AngleGet();
miyajitakenari 6:326208aabe68 81 }
ushiroji 14:3e7d563538e5 82 }
ushiroji 14:3e7d563538e5 83 //角度調節終了
ushiroji 14:3e7d563538e5 84
ushiroji 14:3e7d563538e5 85 //速度変更開始
ushiroji 14:3e7d563538e5 86 xbee.printf("speed flag=");
ushiroji 14:3e7d563538e5 87 wait(3);
ushiroji 14:3e7d563538e5 88 float as[2];//advance speed
miyajitakenari 0:79033ee3c961 89 if(xbee.readable()){
miyajitakenari 0:79033ee3c961 90 xbee.printf("advance speed=");
miyajitakenari 6:326208aabe68 91 xbee.scanf("%f",&as[1]);
ushiroji 14:3e7d563538e5 92 }
ushiroji 14:3e7d563538e5 93 else{
ushiroji 14:3e7d563538e5 94 as[1]=0.5;
ushiroji 14:3e7d563538e5 95 }
ushiroji 14:3e7d563538e5 96 Move('2', as[1]);
ushiroji 14:3e7d563538e5 97 xbee.printf("mortor mode:2 speed:%f",as[1]);
ushiroji 14:3e7d563538e5 98 catchGPS();
ushiroji 14:3e7d563538e5 99 xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
ushiroji 14:3e7d563538e5 100 //速度変更終了
miyajitakenari 0:79033ee3c961 101
ushiroji 14:3e7d563538e5 102 //座標判定
ushiroji 14:3e7d563538e5 103 double lati = 111132.8715; //1度あたりの緯度の距離(m)
ushiroji 14:3e7d563538e5 104 double longi = 91535.79099; //1度あたりの経度の距離(m)
ushiroji 14:3e7d563538e5 105 GPS_x = gps.latitude;
ushiroji 14:3e7d563538e5 106 GPS_y = gps.longitude;
miyajitakenari 4:975b0d9bd51b 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:79033ee3c961 108 xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
miyajitakenari 0:79033ee3c961 109 break;
miyajitakenari 0:79033ee3c961 110 }
ushiroji 14:3e7d563538e5 111 }//while(1){}
ushiroji 14:3e7d563538e5 112 }//for(){}
miyajitakenari 0:79033ee3c961 113 // 行動フロー終了
miyajitakenari 0:79033ee3c961 114 xbee.printf("End\r\n");
ushiroji 14:3e7d563538e5 115 Move('1', 0);
miyajitakenari 0:79033ee3c961 116 return 0;
miyajitakenari 0:79033ee3c961 117 }