narumi tatsuya
ルンパッパ
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "TB6612.h" 00003 #include "getGPS.h" 00004 #include "BMP180.h" 00005 #include <stdio.h> 00006 #include <math.h> 00007 00008 #define PIN_SDA D4 00009 #define PIN_SCL D5 00010 00011 Serial pc(SERIAL_TX,SERIAL_RX,921600);//通信 00012 BMP180 bmp180(PIN_SDA,PIN_SCL);//気圧センサーのピン 00013 Serial xbee(D1,D0);//Xbeeのピン 00014 DigitalOut FET1(D9);//FETのピン 00015 DigitalOut FET2(D8); 00016 DigitalIn flight(D6); //フライトピンのピン 00017 DigitalOut SW(D7); 00018 TB6612 motor(D7,D9,D11);//モータードライバーのピン 00019 GPS gps (D13,A7); 00020 00021 00022 int main(){ 00023 float avalt,x8; 00024 float pressure,temperature,altitude;//気圧，気温，高度 00025 FET1=0; 00026 FET2=1; 00027 wait(10); 00028 FET2=0; 00029 FET1=1; 00030 wait(10); 00031 FET1=0; 00032 FET2=1; 00033 wait(10); 00034 00035 SW=1; 00036 flight==1;//フライトピンがついている 00037 00038 while(1) { 00039 if(flight==1) { 00040 wait(1); 00041 }//フライトピンがついているとき1秒待機 00042 00043 else{ 00044 if(flight==1) { 00045 wait(1); 00046 } 00047 else{ 00048 00049 SW = 0; 00050 FET2=1; 00051 wait(25); 00052 xbee.printf("やったぞおおおおおおおおお!\n"); 00053 00054 break; 00055 } 00056 } 00057 } 00058 00059 float avalt,x8; 00060 float pressure,temperature,altitude;//気圧，気温，高度 00061 int t=0; 00062 00063 xbee.printf("\rstart!\n\r");//気圧センサースタート 00064 bmp180.Initialize(27,BMP180_OSS_ULTRA_HIGH_RESOLUTION);//27は府大の海抜高度 00065 xbee.printf("initialization complete!\n\r");//初期化完了 00066 00067 while(1){ 00068 float x4,x5,x6,x7,a,b; 00069 float sum=0,altitude[11]; 00070 int r=0; 00071 for(int p=1;p<11;p++){ 00072 if(bmp180.ReadData(&temperature,&pressure)){ 00073 a = pressure; 00074 b = temperature; 00075 x4 = 1019.11 / a; //海面気圧を気圧でわる 00076 x5 = powf(x4, 0.1902225); //5.257ぶんの1 00077 x6 = 273.15 + b; //絶対温度 00078 x7 = (x5 - 1) * x6; 00079 x8 = x7 / 0.0065; 00080 altitude[p] = x8; 00081 sum=sum+altitude[p]; 00082 r++; 00083 xbee.printf("altitude(m)\t:%.3f\n\r",altitude[p]); 00084 xbee.printf("--------------------------------\n\r"); 00085 }else{ 00086 xbee.printf("NO DATA\n\r"); 00087 xbee.printf("---------------------------\n\r"); 00088 wait(1); 00089 } 00090 } 00091 avalt=sum/(float)r; 00092 xbee.printf("Altitude(m)\t:%.3f\n\r",avalt); 00093 xbee.printf("--------------------------------\n\r"); 00094 wait(3); 00095 break; 00096 } 00097 while(1){ 00098 float y4,y5,y6,y7,c,d; 00099 float y8=avalt; 00100 float speed; 00101 float s=0; 00102 float alti[128]; 00103 float average_alti; 00104 int e=0; 00105 int i=1; 00106 00107 for(int q=1;q<11;q++){ 00108 if(bmp180.ReadData(&temperature,&pressure)){ 00109 c = pressure; 00110 d = temperature; 00111 y4 = 1019.11 / c; //海面気圧を気圧でわる 00112 y5 = powf(y4,0.1902225); 00113 y6 = 273.15 + d; 00114 y7 = (y5 - 1) * y6; 00115 y8 = y7 / 0.0065; 00116 alti[q]= y8; 00117 s=s+alti[q]; 00118 xbee.printf("高度%d回目%f\n\r",q-i,alti[q]); 00119 xbee.printf("-------------------------------\n\r"); 00120 e++; 00121 }else{ 00122 xbee.printf("NO_Data\n\r"); 00123 } 00124 } 00125 average_alti=s/(float)e; 00126 speed = (avalt - average_alti)/3; 00127 00128 xbee.printf("Altitude(m)\t:%.3f\n\r",average_alti); 00129 xbee.printf("Speed(m/s)\t:%.3f\n\r",speed); 00130 xbee.printf("-------------------------------\n\r"); 00131 00132 avalt=average_alti; 00133 xbee.printf("tの値は%d\n",t); 00134 i++; 00135 if(speed<=0.5){ 00136 t=t+1; 00137 }if(t==3){ 00138 break; 00139 } 00140 wait(3); 00141 } 00142 } 00143 00144 00145 /*speedが3回0.5m/s以下になったらFETに20秒電流を流してその後電流を止める*/ 00146 FET1=1; 00147 wait(10); 00148 FET1=0; 00149 motor = 100; 00150 double a; 00151 double b; 00152 double distance; 00153 00154 xbee.printf("GPS begin\n"); 00155 00156 while(1){ 00157 if(gps.getgps()){ 00158 /*a,bを緯度経度の初期値で初期化*/ 00159 a=gps.latitude; 00160 b=gps.longitude; 00161 xbee.printf("(%lf,%lf)\r\n",a,b);//緯度と経度を表示 00162 xbee.printf("--------------------------------\n\r"); 00163 break; 00164 }else{ 00165 xbee.printf("Fault_No_Data\r\n"); 00166 wait(1); 00167 } 00168 } 00169 while(1){ 00170 if(gps.getgps()){ 00171 xbee.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示 00172 xbee.printf("--------------------------------\n\r"); 00173 00174 /*ここから距離の計算*/ 00175 /*c、dを得た緯度経度の値で初期化*/ 00176 double c; 00177 double d; 00178 c=gps.latitude; 00179 d=gps.longitude; 00180 00181 const double pi=3.14159265359;//円周率 00182 00183 /*ラジアンに変換*/ 00184 double theta_a=a*pi/180; 00185 double theta_b=b*pi/180; 00186 double theta_c=c*pi/180; 00187 double theta_d=d*pi/180; 00188 00189 double e=sin(theta_a)*sin(theta_c)+cos(theta_a)*cos(theta_c)*cos(theta_b-theta_d);//２点間のなす角を求める 00190 double theta_r=acos(e); 00191 00192 const double earth_radius=6378140;//赤道半径 00193 00194 distance=earth_radius*theta_r;//距離の計算 00195 00196 /*距離が30m以上なら表示、通信*/ 00197 if(distance>=30){ 00198 pc.printf("run over 20m"); 00199 motor=0; 00200 xbee.printf("run over 20m"); 00201 break; 00202 } 00203 00204 }else { 00205 xbee.printf("False_No_Data\r\n"); 00206 pc.printf("False_No_Date\r\n"); 00207 wait(1); 00208 }//データ取得失敗を表示、通信、1秒待機 00209 00210 } 00211 xbee.printf("成功\n"); 00212 return 0; 00213 }
Generated on Wed Jul 13 2022 20:56:16 by
1.7.2