narumi tatsuya / Mbed 2 deprecated motor_sample

ルンパッパ

Dependencies:   mbed BMP180

main.cpp@1:1dab888e1f3c, 2020-12-16 (annotated)

Committer:
naruu
Date:
Wed Dec 16 07:18:54 2020 +0000
Revision:
1:1dab888e1f3c
Parent:
0:5cddbcb7193f
Child:
2:20304b3cea67
runpappa;

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