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