narumi tatsuya / Mbed 2 deprecated motor_sample

ルンパッパ

Dependencies:   mbed BMP180

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?

UserRevisionLine numberNew 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);//２点間のなす角を求める
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 }