CanSat-C 2021 / Mbed 2 deprecated test

テスト用　gps手入力

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

main.cpp@0:f7517c11d468, 2021-11-11 (annotated)

Committer:
miyajitakenari
Date:
Thu Nov 11 09:41:22 2021 +0000
Revision:
0:f7517c11d468
Child:
1:41dfafb10c80
test you

Who changed what in which revision?

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