narumi tatsuya / Mbed 2 deprecated motor_sample

ルンパッパ

Dependencies:   mbed BMP180

main.cpp@3:3a62c5ccfddb, 2020-12-18 (annotated)

Committer:
naruu
Date:
Fri Dec 18 11:43:47 2020 +0000
Revision:
3:3a62c5ccfddb
Parent:
2:20304b3cea67
Child:
4:443e82b039ba
kouo;

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