narumi tatsuya
/
mou
ニョロゾ
main.cpp
- Committer:
- naruu
- Date:
- 2020-12-17
- Revision:
- 2:98629c0bb9ff
- Parent:
- 1:7382df606336
- Child:
- 3:258c138c6299
File content as of revision 2:98629c0bb9ff:
#include "mbed.h" #include "TB6612.h" #include "getGPS.h" #include "BMP180.h" #include <stdio.h> #include <math.h> #define PIN_SDA D4 #define PIN_SCL D5 Serial pc(SERIAL_TX,SERIAL_RX,921600);//通信 Serial xbee(D1,D0);//Xbeeのピン DigitalOut FET(D9);//FETのピン DigitalIn flight(D6); //フライトピンのピン DigitalOut SW(D7);//フライトピンの電圧降下ピン TB6612 motor(D7,D9,D11);//モータードライバーのピン GPS gps (D1,D0); int main(){ float x8; FET=0; SW=1; flight==1;//フライトピンがついている while(1) { if(flight==1) { wait(1); }//フライトピンがついているとき1秒待機 else{ if(flight==1) { wait(1); } else{ SW = 0; pc.printf("やったぞおおおおおおおおお!\n"); break; } } } BMP180 bmp180(PIN_SDA,PIN_SCL); float pressure,temperature,altitude;//気圧,気温,高度 int n; pc.printf("\rstart!\n\r");//気圧センサースタート bmp180.Initialize(27,BMP180_OSS_ULTRA_HIGH_RESOLUTION);//27は府大の海抜高度 pc.printf("initialization complete!\n\r");//初期化完了 while(1){ if(bmp180.ReadData(&temperature,&pressure)){ float x4,x5,x6,x7,a,b; a = pressure; b = temperature; x4 = 1019.11 / a; //海面気圧を気圧でわる x5 = powf(x4, 0.1902225); //5.257ぶんの1 x6 = 273.15 + b; //絶対温度 x7 = (x5 - 1) * x6; x8 = x7 / 0.0065; altitude = x8; pc.printf("Altitude(m)\t:%.3f\n\r",altitude); pc.printf("--------------------------------\n\r"); wait(3); break; n=0; }else{ pc.printf("NO DATA\n\r"); pc.printf("---------------------------\n\r"); wait(1); } } while(1){ if(bmp180.ReadData(&temperature,&pressure)){ float y4,y5,y6,y7,y8,c,d; float speed; c = pressure; d = temperature; y4 = 1019.11 / c; //海面気圧を気圧でわる y5 = powf(y4,0.1902225); y6 = 273.15 + d; y7 = (y5 - 1) * y6; y8 = y7 / 0.0065; altitude = y8; speed = (x8 - y8)/(float)(3+n);//値が取得でた場合は,3秒間の速さをだし,値が取得できなかった場合は3+n秒(nは値が取得できなかった回数)の速さをだす pc.printf("Altitude(m)\t:%.3f\n\r",altitude); pc.printf("Speed(m/s)\t:%.3f\n\r",speed); pc.printf("--------------------------------\n\r"); x8 = y8; n=0; wait(3); if(speed<=0){ break; } }else{ pc.printf("NO DATA\n\r"); ++n; wait(1); } } /*speedが0以下になったらFETに20秒電流を流してその後電流を止める*/ /* FET=1; wait(20); FET=0;*/ motor = 100; double a; double b; double distance; pc.printf("GPS begin\n"); while(1){ if(gps.getgps()){ /*a,bを緯度経度の初期値で初期化*/ a=gps.latitude; b=gps.longitude; pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示 pc.printf("--------------------------------\n\r"); break; }else{ pc.printf("Fault_No_Data\r\n"); wait(1); } } while(1){ if(gps.getgps()){ pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示 pc.printf("--------------------------------\n\r"); /*ここから距離の計算*/ /*c、dを得た緯度経度の値で初期化*/ double c; double d; c=gps.latitude; d=gps.longitude; const double pi=3.14159265359;//円周率 /*ラジアンに変換*/ double theta_a=a*pi/180; double theta_b=b*pi/180; double theta_c=c*pi/180; double theta_d=d*pi/180; double e=sin(theta_a)*sin(theta_c)+cos(theta_a)*cos(theta_c)*cos(theta_b-theta_d);//2点間のなす角を求める double theta_r=acos(e); const double earth_radius=6378140;//赤道半径 distance=earth_radius*theta_r;//距離の計算 /*距離が25m以上なら表示、通信*/ if(distance>=30){ pc.printf("run over 20m"); motor=0; pc.printf("run over 20m"); break; } }else { pc.printf("False_No_Data\r\n"); pc.printf("False_No_Date\r\n"); wait(1); }//データ取得失敗を表示、通信、1秒待機 } pc.printf("成功\n"); return 0; }