![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ニョロゾ
main.cpp@3:258c138c6299, 2020-12-17 (annotated)
- Committer:
- naruu
- Date:
- Thu Dec 17 17:36:33 2020 +0000
- Revision:
- 3:258c138c6299
- Parent:
- 2:98629c0bb9ff
- Child:
- 4:e84fb33d8c2b
kousaoifj;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
naruu | 0:04440e6b70fc | 1 | #include "mbed.h" |
naruu | 2:98629c0bb9ff | 2 | #include "TB6612.h" |
naruu | 2:98629c0bb9ff | 3 | #include "getGPS.h" |
naruu | 2:98629c0bb9ff | 4 | #include "BMP180.h" |
naruu | 2:98629c0bb9ff | 5 | #include <stdio.h> |
naruu | 2:98629c0bb9ff | 6 | #include <math.h> |
naruu | 2:98629c0bb9ff | 7 | |
naruu | 2:98629c0bb9ff | 8 | #define PIN_SDA D4 |
naruu | 2:98629c0bb9ff | 9 | #define PIN_SCL D5 |
naruu | 0:04440e6b70fc | 10 | |
naruu | 2:98629c0bb9ff | 11 | Serial pc(SERIAL_TX,SERIAL_RX,921600);//通信 |
naruu | 2:98629c0bb9ff | 12 | Serial xbee(D1,D0);//Xbeeのピン |
naruu | 2:98629c0bb9ff | 13 | DigitalOut FET(D9);//FETのピン |
naruu | 2:98629c0bb9ff | 14 | DigitalIn flight(D6); //フライトピンのピン |
naruu | 2:98629c0bb9ff | 15 | DigitalOut SW(D7);//フライトピンの電圧降下ピン |
naruu | 2:98629c0bb9ff | 16 | TB6612 motor(D7,D9,D11);//モータードライバーのピン |
naruu | 2:98629c0bb9ff | 17 | GPS gps (D1,D0); |
naruu | 0:04440e6b70fc | 18 | |
naruu | 2:98629c0bb9ff | 19 | |
naruu | 2:98629c0bb9ff | 20 | int main(){ |
naruu | 2:98629c0bb9ff | 21 | float x8; |
naruu | 2:98629c0bb9ff | 22 | FET=0; |
naruu | 2:98629c0bb9ff | 23 | SW=1; |
naruu | 2:98629c0bb9ff | 24 | flight==1;//フライトピンがついている |
naruu | 3:258c138c6299 | 25 | motor=0; |
naruu | 2:98629c0bb9ff | 26 | |
naruu | 0:04440e6b70fc | 27 | |
naruu | 0:04440e6b70fc | 28 | while(1) { |
naruu | 0:04440e6b70fc | 29 | if(flight==1) { |
naruu | 2:98629c0bb9ff | 30 | wait(1); |
naruu | 2:98629c0bb9ff | 31 | }//フライトピンがついているとき1秒待機 |
naruu | 0:04440e6b70fc | 32 | |
naruu | 0:04440e6b70fc | 33 | else{ |
naruu | 0:04440e6b70fc | 34 | if(flight==1) { |
naruu | 2:98629c0bb9ff | 35 | wait(1); |
naruu | 0:04440e6b70fc | 36 | } |
naruu | 0:04440e6b70fc | 37 | else{ |
naruu | 2:98629c0bb9ff | 38 | |
naruu | 0:04440e6b70fc | 39 | SW = 0; |
naruu | 2:98629c0bb9ff | 40 | pc.printf("やったぞおおおおおおおおお!\n"); |
naruu | 2:98629c0bb9ff | 41 | |
naruu | 0:04440e6b70fc | 42 | break; |
naruu | 2:98629c0bb9ff | 43 | } |
naruu | 0:04440e6b70fc | 44 | } |
naruu | 0:04440e6b70fc | 45 | } |
naruu | 2:98629c0bb9ff | 46 | BMP180 bmp180(PIN_SDA,PIN_SCL); |
naruu | 2:98629c0bb9ff | 47 | float pressure,temperature,altitude;//気圧,気温,高度 |
naruu | 2:98629c0bb9ff | 48 | int n; |
naruu | 2:98629c0bb9ff | 49 | pc.printf("\rstart!\n\r");//気圧センサースタート |
naruu | 2:98629c0bb9ff | 50 | bmp180.Initialize(27,BMP180_OSS_ULTRA_HIGH_RESOLUTION);//27は府大の海抜高度 |
naruu | 2:98629c0bb9ff | 51 | pc.printf("initialization complete!\n\r");//初期化完了 |
naruu | 2:98629c0bb9ff | 52 | |
naruu | 2:98629c0bb9ff | 53 | while(1){ |
naruu | 2:98629c0bb9ff | 54 | if(bmp180.ReadData(&temperature,&pressure)){ |
naruu | 2:98629c0bb9ff | 55 | float x4,x5,x6,x7,a,b; |
naruu | 2:98629c0bb9ff | 56 | a = pressure; |
naruu | 2:98629c0bb9ff | 57 | b = temperature; |
naruu | 2:98629c0bb9ff | 58 | x4 = 1019.11 / a; //海面気圧を気圧でわる |
naruu | 2:98629c0bb9ff | 59 | x5 = powf(x4, 0.1902225); //5.257ぶんの1 |
naruu | 2:98629c0bb9ff | 60 | x6 = 273.15 + b; //絶対温度 |
naruu | 2:98629c0bb9ff | 61 | x7 = (x5 - 1) * x6; |
naruu | 2:98629c0bb9ff | 62 | x8 = x7 / 0.0065; |
naruu | 2:98629c0bb9ff | 63 | altitude = x8; |
naruu | 2:98629c0bb9ff | 64 | |
naruu | 2:98629c0bb9ff | 65 | |
naruu | 2:98629c0bb9ff | 66 | pc.printf("Altitude(m)\t:%.3f\n\r",altitude); |
naruu | 2:98629c0bb9ff | 67 | pc.printf("--------------------------------\n\r"); |
naruu | 2:98629c0bb9ff | 68 | wait(3); |
naruu | 2:98629c0bb9ff | 69 | break; |
naruu | 2:98629c0bb9ff | 70 | n=0; |
naruu | 2:98629c0bb9ff | 71 | }else{ |
naruu | 2:98629c0bb9ff | 72 | pc.printf("NO DATA\n\r"); |
naruu | 2:98629c0bb9ff | 73 | pc.printf("---------------------------\n\r"); |
naruu | 2:98629c0bb9ff | 74 | wait(1); |
naruu | 2:98629c0bb9ff | 75 | } |
naruu | 2:98629c0bb9ff | 76 | } |
naruu | 2:98629c0bb9ff | 77 | while(1){ |
naruu | 2:98629c0bb9ff | 78 | if(bmp180.ReadData(&temperature,&pressure)){ |
naruu | 2:98629c0bb9ff | 79 | float y4,y5,y6,y7,y8,c,d; |
naruu | 2:98629c0bb9ff | 80 | float speed; |
naruu | 2:98629c0bb9ff | 81 | |
naruu | 2:98629c0bb9ff | 82 | c = pressure; |
naruu | 2:98629c0bb9ff | 83 | d = temperature; |
naruu | 2:98629c0bb9ff | 84 | y4 = 1019.11 / c; //海面気圧を気圧でわる |
naruu | 2:98629c0bb9ff | 85 | y5 = powf(y4,0.1902225); |
naruu | 2:98629c0bb9ff | 86 | y6 = 273.15 + d; |
naruu | 2:98629c0bb9ff | 87 | y7 = (y5 - 1) * y6; |
naruu | 2:98629c0bb9ff | 88 | y8 = y7 / 0.0065; |
naruu | 2:98629c0bb9ff | 89 | altitude = y8; |
naruu | 2:98629c0bb9ff | 90 | speed = (x8 - y8)/(float)(3+n);//値が取得でた場合は,3秒間の速さをだし,値が取得できなかった場合は3+n秒(nは値が取得できなかった回数)の速さをだす |
naruu | 2:98629c0bb9ff | 91 | |
naruu | 2:98629c0bb9ff | 92 | pc.printf("Altitude(m)\t:%.3f\n\r",altitude); |
naruu | 2:98629c0bb9ff | 93 | pc.printf("Speed(m/s)\t:%.3f\n\r",speed); |
naruu | 2:98629c0bb9ff | 94 | pc.printf("--------------------------------\n\r"); |
naruu | 2:98629c0bb9ff | 95 | x8 = y8; |
naruu | 2:98629c0bb9ff | 96 | n=0; |
naruu | 2:98629c0bb9ff | 97 | wait(3); |
naruu | 2:98629c0bb9ff | 98 | if(speed<=0){ |
naruu | 2:98629c0bb9ff | 99 | break; |
naruu | 2:98629c0bb9ff | 100 | } |
naruu | 2:98629c0bb9ff | 101 | }else{ |
naruu | 2:98629c0bb9ff | 102 | pc.printf("NO DATA\n\r"); |
naruu | 2:98629c0bb9ff | 103 | ++n; |
naruu | 2:98629c0bb9ff | 104 | wait(1); |
naruu | 2:98629c0bb9ff | 105 | } |
naruu | 2:98629c0bb9ff | 106 | } |
naruu | 2:98629c0bb9ff | 107 | /*speedが0以下になったらFETに20秒電流を流してその後電流を止める*/ |
naruu | 2:98629c0bb9ff | 108 | /* FET=1; |
naruu | 2:98629c0bb9ff | 109 | wait(20); |
naruu | 2:98629c0bb9ff | 110 | FET=0;*/ |
naruu | 2:98629c0bb9ff | 111 | motor = 100; |
naruu | 2:98629c0bb9ff | 112 | double a; |
naruu | 2:98629c0bb9ff | 113 | double b; |
naruu | 2:98629c0bb9ff | 114 | double distance; |
naruu | 2:98629c0bb9ff | 115 | |
naruu | 2:98629c0bb9ff | 116 | pc.printf("GPS begin\n"); |
naruu | 2:98629c0bb9ff | 117 | |
naruu | 2:98629c0bb9ff | 118 | while(1){ |
naruu | 2:98629c0bb9ff | 119 | if(gps.getgps()){ |
naruu | 2:98629c0bb9ff | 120 | /*a,bを緯度経度の初期値で初期化*/ |
naruu | 2:98629c0bb9ff | 121 | a=gps.latitude; |
naruu | 2:98629c0bb9ff | 122 | b=gps.longitude; |
naruu | 2:98629c0bb9ff | 123 | pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示 |
naruu | 2:98629c0bb9ff | 124 | pc.printf("--------------------------------\n\r"); |
naruu | 2:98629c0bb9ff | 125 | break; |
naruu | 2:98629c0bb9ff | 126 | }else{ |
naruu | 2:98629c0bb9ff | 127 | pc.printf("Fault_No_Data\r\n"); |
naruu | 2:98629c0bb9ff | 128 | wait(1); |
naruu | 2:98629c0bb9ff | 129 | } |
naruu | 2:98629c0bb9ff | 130 | } |
naruu | 2:98629c0bb9ff | 131 | while(1){ |
naruu | 2:98629c0bb9ff | 132 | if(gps.getgps()){ |
naruu | 2:98629c0bb9ff | 133 | pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示 |
naruu | 2:98629c0bb9ff | 134 | pc.printf("--------------------------------\n\r"); |
naruu | 2:98629c0bb9ff | 135 | |
naruu | 2:98629c0bb9ff | 136 | /*ここから距離の計算*/ |
naruu | 2:98629c0bb9ff | 137 | /*c、dを得た緯度経度の値で初期化*/ |
naruu | 2:98629c0bb9ff | 138 | double c; |
naruu | 2:98629c0bb9ff | 139 | double d; |
naruu | 2:98629c0bb9ff | 140 | c=gps.latitude; |
naruu | 2:98629c0bb9ff | 141 | d=gps.longitude; |
naruu | 2:98629c0bb9ff | 142 | |
naruu | 2:98629c0bb9ff | 143 | const double pi=3.14159265359;//円周率 |
naruu | 2:98629c0bb9ff | 144 | |
naruu | 2:98629c0bb9ff | 145 | /*ラジアンに変換*/ |
naruu | 2:98629c0bb9ff | 146 | double theta_a=a*pi/180; |
naruu | 2:98629c0bb9ff | 147 | double theta_b=b*pi/180; |
naruu | 2:98629c0bb9ff | 148 | double theta_c=c*pi/180; |
naruu | 2:98629c0bb9ff | 149 | double theta_d=d*pi/180; |
naruu | 2:98629c0bb9ff | 150 | |
naruu | 2:98629c0bb9ff | 151 | double e=sin(theta_a)*sin(theta_c)+cos(theta_a)*cos(theta_c)*cos(theta_b-theta_d);//2点間のなす角を求める |
naruu | 2:98629c0bb9ff | 152 | double theta_r=acos(e); |
naruu | 2:98629c0bb9ff | 153 | |
naruu | 2:98629c0bb9ff | 154 | const double earth_radius=6378140;//赤道半径 |
naruu | 2:98629c0bb9ff | 155 | |
naruu | 2:98629c0bb9ff | 156 | distance=earth_radius*theta_r;//距離の計算 |
naruu | 2:98629c0bb9ff | 157 | |
naruu | 2:98629c0bb9ff | 158 | /*距離が25m以上なら表示、通信*/ |
naruu | 2:98629c0bb9ff | 159 | if(distance>=30){ |
naruu | 2:98629c0bb9ff | 160 | pc.printf("run over 20m"); |
naruu | 2:98629c0bb9ff | 161 | motor=0; |
naruu | 2:98629c0bb9ff | 162 | pc.printf("run over 20m"); |
naruu | 2:98629c0bb9ff | 163 | break; |
naruu | 2:98629c0bb9ff | 164 | } |
naruu | 2:98629c0bb9ff | 165 | |
naruu | 2:98629c0bb9ff | 166 | }else { |
naruu | 2:98629c0bb9ff | 167 | pc.printf("False_No_Data\r\n"); |
naruu | 2:98629c0bb9ff | 168 | pc.printf("False_No_Date\r\n"); |
naruu | 2:98629c0bb9ff | 169 | wait(1); |
naruu | 2:98629c0bb9ff | 170 | }//データ取得失敗を表示、通信、1秒待機 |
naruu | 2:98629c0bb9ff | 171 | |
naruu | 2:98629c0bb9ff | 172 | } |
naruu | 2:98629c0bb9ff | 173 | pc.printf("成功\n"); |
naruu | 2:98629c0bb9ff | 174 | return 0; |
naruu | 0:04440e6b70fc | 175 | } |