テスト用 gps手入力
Dependencies: ATP3012 mbed a HMC US015_2 getGPS
Diff: main.cpp
- Revision:
- 0:f7517c11d468
- Child:
- 1:41dfafb10c80
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Nov 11 09:41:22 2021 +0000 @@ -0,0 +1,113 @@ +/*ライブラリ*/ +#include "mbed.h" + +// 自作関数 +#include "Function.h" + +// フライトピン・ニクロム線関係 +DigitalIn flight_pin(A0); +DigitalOut nichrome(D13); +// +#define cp_max 3 //CPの数を入力する + +int main() { + // 変数宣言 + double GPS_x, GPS_y; // 現在地の座標 + double direction; // 次CPへの向き + double CPs_x[3]={1,2,3}; //CPリスト(x座標) + double CPs_y[3]={1,2,3}; // CPリスト(y座標) + double next_CP_x, next_CP_y; + + // 落下検知 + // パラシュート分離 + + wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん + while(flight_pin){} + xbee.printf("flight_pin nuketa"); + wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s + nichrome=1; + xbee.printf("nichrome in"); + wait(30); + nichrome=0; + // 落下終了 + + + // 行動フロー開始 + Calibration(); + xbee.printf("XBee Connected\r\n"); + xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude); + for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動 + next_CP_x = CPs_x[i]; + next_CP_y = CPs_y[i]; + xbee.printf("next_i=%d\r\n", i); + + while (1) { + speak(); + direction = AngleGet(); + xbee.printf("direction=%f\n\rdirection start", direction); + int df=1; + //角度調節 + while(1) { + if(direction < 5 || direction > 355) { //角度判定 + xbee.printf("direction finish\n\r"); + break; + } + else { + Move('1', 0);//停止 + if(df==1){ + xbee.printf("mortor mode:1 speed:0\n\r"); + } + Move('4', 0.5);//時計回りに回転 + if(df==1){ + xbee.printf("mortor mode:4 speed:0.5\n\r"); + df++; + direction = AngleGet(); + break; + } + } + } + while(FrontGet()) { + xbee.printf("front get\n\r"); + Move('1', 0); //停止 + xbee.printf("mortor mode:1 speed:0\n\r"); + Move('4', 0.5); //時計回り回転 + xbee.printf("mortor mode:4 speed:0.5\n\r"); + wait(1); + Move('1', 0); //回転停止 + xbee.printf("mortor mode:1 speed:0\n\r"); + } + xbee.printf("speed flag="); + wait(5); + float as[2];//advance speed + if(xbee.readable()){ + xbee.printf("advance speed="); + xbee.scanf("%f",&as[1]); + }else{ + as[1]=0.5; + } + Move('2', as[1]); + xbee.printf("mortor mode:2 speed:%f",as[1]); + catchGPS(); + xbee.printf("GPS_x=xbee input"); + xbee.scanf("%lf",&GPS_x); + xbee.printf("GPS_y=xbee input"); + xbee.scanf("%lf",&GPS_y); + xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude); + + double lati = 111132.8715; //1度あたりの緯度の距離(m) + double longi = 91535.79099; //1度あたりの経度の距離(m) + //GPS_x = gps.latitude; + //GPS_y = gps.longitude; + 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到着判定 //試験で調整 + xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y); + break; + } + + }//while(1){} + }//for(){} + // 行動フロー終了 + xbee.printf("End\r\n"); + Move('1', 0); //停止 + xbee.printf("mortor mode:1 speed:0"); + return 0; +}