narumi tatsuya
/
go-went-Carlos_Gone
ニドキング
Diff: main.cpp
- Revision:
- 1:1dab888e1f3c
- Parent:
- 0:5cddbcb7193f
- Child:
- 2:20304b3cea67
--- a/main.cpp Tue Dec 15 08:36:33 2020 +0000 +++ b/main.cpp Wed Dec 16 07:18:54 2020 +0000 @@ -1,7 +1,169 @@ #include "mbed.h" #include "TB6612.h" -TB6612 motor(D7,D9,D11); -int main(){ - motor=100; +#include "getGPS.h" +#include"math.h" +#include "BMP180.h" +#include <stdio.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);//モータードライバーのピン + + + 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; + xbee.printf("やったぞおおおおおおおおお!\n"); + pc.printf("やったぞおおおおおおおおお!\n");//フライトピンが外れたときに表示 + break; + } + } + } + BMP180 bmp180(PIN_SDA,PIN_SCL); + float pressure,temperature,altitude;//気圧,気温,高度 + xbee.printf("\rstart!\n\r");//気圧センサースタート + bmp180.Initialize(27,BMP180_OSS_ULTRA_HIGH_RESOLUTION);//27は府大の海抜高度 + xbee.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; + + + xbee.printf("Altitude(m)\t:%.3f\n\r",altitude); + xbee.printf("--------------------------------\n\r"); + wait(1); + break; + }else{ + xbee.printf("NO DATA\n\r"); + xbee.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)/1; + + xbee.printf("Altitude(m)\t:%.3f\n\r",altitude); + xbee.printf("Speed(m/s)\t:%.3f\n\r",speed); + xbee.printf("--------------------------------\n\r"); + x8 = y8; + wait(1); + if(speed<=0){ + break; + }} + else{ + xbee.printf("NO DATA\n\r"); + wait(1); + } + } + /*speedが0以下になったらFETに20秒電流を流してその後電流を止まる*/ + FET=1; +wait(20); +FET=0; + +} + //motor=100; + + double a; + double b; + double distance; + + xbee.printf("GPS begin\n");; + 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);//緯度と経度を表示 + 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);//緯度と経度を表示 + + /*ここから距離の計算*/ + /*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;//距離の計算 + + /*距離が20m以上なら表示、通信*/ + if(distance>=25){ + pc.printf("run over20m"); + motor=0; + xbee.printf("run over20m"); + break; + } + + }else { + xbee.printf("False_No_Data\r\n"); + pc.printf("False_No_Date\r\n"); + wait(1); + }//データ取得失敗を表示、通信、1秒待機 + + } + return 0; } \ No newline at end of file