narumi tatsuya / Mbed 2 deprecated mou

ニョロゾ

Dependencies:   mbed BMP180

main.cpp@6:f862230de7b5, 2020-12-18 (annotated)

Committer:
naruu
Date:
Fri Dec 18 02:31:39 2020 +0000
Revision:
6:f862230de7b5
Parent:
5:eedb48931268 
kou;

Who changed what in which revision?

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