narumi tatsuya
/
flightpin
ロロロロ
Revision 2:98629c0bb9ff, committed 2020-12-17
- Comitter:
- naruu
- Date:
- Thu Dec 17 17:29:28 2020 +0000
- Parent:
- 1:7382df606336
- Commit message:
- no;
Changed in this revision
diff -r 7382df606336 -r 98629c0bb9ff BMP180.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BMP180.lib Thu Dec 17 17:29:28 2020 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/spiridion/code/BMP180/#072073c79cfd
diff -r 7382df606336 -r 98629c0bb9ff TB6612.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TB6612.cpp Thu Dec 17 17:29:28 2020 +0000 @@ -0,0 +1,57 @@ +/** + * Motor Driver TB6612 Control Library + * + * -- TB6612 is a device of the TOSHIBA. + * + * Copyright (C) 2012 Junichi Katsu (JKSOFT) + */ + + +#include "TB6612.h" + +// TB6612 Class Constructor +TB6612::TB6612(PinName pwm, PinName fwd, PinName rev): + _pwm(pwm), _fwd(fwd), _rev(rev) { + + _fwd = 0; + _rev = 0; + _pwm = 0.0; + _pwm.period(0.001); +} + +// Speed Control +// arg +// int speed -100 -- 0 -- 100 +void TB6612::speed(int speed) { + + if( speed > 0 ) + { + _pwm = ((float)speed) / 100.0; + _fwd = 1; + _rev = 0; + } + else if( speed < 0 ) + { + _pwm = -((float)speed) / 100.0; + _fwd = 0; + _rev = 1; + } + else + { + _fwd = 1; + _rev = 1; + } +} + + +// Speed Control with time-out +// arg +// int speed -100 -- 0 -- 100 +// int time 0 +void TB6612::move(int sspeed , int time) +{ + speed(sspeed); + wait_ms(time); +} + + \ No newline at end of file
diff -r 7382df606336 -r 98629c0bb9ff TB6612.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TB6612.h Thu Dec 17 17:29:28 2020 +0000 @@ -0,0 +1,30 @@ +/** + * Motor Driver TB6612 Control Library + * + * -- TB6612 is a device of the rohm. + * + * Copyright (C) 2012 Junichi Katsu (JKSOFT) + */ + +#ifndef MBED_TB6612_H +#define MBED_TB6612_H + +#include "mbed.h" + +class TB6612 { +public: + TB6612(PinName pwm, PinName fwd, PinName rev); + void speed(int speed); + void move(int speed , int time); + void operator= ( int value ) + { + speed(value); + } + +protected: + PwmOut _pwm; + DigitalOut _fwd; + DigitalOut _rev; +}; + +#endif \ No newline at end of file
diff -r 7382df606336 -r 98629c0bb9ff getGPS.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/getGPS.cpp Thu Dec 17 17:29:28 2020 +0000 @@ -0,0 +1,58 @@ +#include "mbed.h" +#include "getGPS.h" + +GPS::GPS(PinName gpstx,PinName gpsrx): _gps(gpstx,gpsrx) +{ + latitude = 0; + longitude = 0; + _gps.baud(GPSBAUD); + _gps.printf("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"); +} + +bool GPS::getgps() +{ + char gps_data[256]; + int i; + + do { + while(_gps.getc() != '$'); //$マークまで読み飛ばし + i = 0; + + /* gpa_data初期化 */ + for(int j = 0; j < 256; j++) + gps_data[j] = '\0'; + + /* NMEAから一行読み込み */ + while((gps_data[i] = _gps.getc()) != '\r') { + i++; + if(i == 256) { + i = 255; + break; + } + } + } while(strstr(gps_data, "GPGGA") == NULL); //GGAセンテンスまで一行ずつ読み込み続ける + + int rlock; + char ns,ew; + double w_time, raw_longitude, raw_latitude; + int satnum; + double hdop; + + if(sscanf(gps_data, "GPGGA, %lf, %lf, %c, %lf, %c, %d, %d, %lf", &w_time, &raw_latitude, &ns, &raw_longitude, &ew, &rlock, &satnum, &hdop) > 1) { + /* 座標1(度部分) */ + double latitude_dd = (double)(raw_latitude / 100); + double longitude_dd = (double)(raw_longitude / 100); + + /* 座標2(分部分 → 度) */ + double latitude_md = (raw_latitude - latitude_dd * 100) / 60; + double longitude_md = (raw_longitude - longitude_dd * 100) / 60; + + /* 座標1 + 2 */ + latitude = latitude_dd + latitude_md; + longitude = longitude_dd + longitude_md; + + return true; + } else + return false; //GGAセンテンスの情報が欠けている時 +} + \ No newline at end of file
diff -r 7382df606336 -r 98629c0bb9ff getGPS.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/getGPS.h Thu Dec 17 17:29:28 2020 +0000 @@ -0,0 +1,20 @@ +#ifndef GPS_H +#define GPS_H + +#define GPSBAUD 9600 //ボーレート + +class GPS { +public: + GPS(PinName gpstx,PinName gpsrx); + + bool getgps(); + + double longitude; + double latitude; + +private: + Serial _gps; +}; + +#endif //GPS_H + \ No newline at end of file
diff -r 7382df606336 -r 98629c0bb9ff main.cpp --- a/main.cpp Fri Nov 06 06:40:01 2020 +0000 +++ b/main.cpp Thu Dec 17 17:29:28 2020 +0000 @@ -1,35 +1,174 @@ #include "mbed.h" +#include "TB6612.h" +#include "getGPS.h" +#include "BMP180.h" +#include <stdio.h> +#include <math.h> + +#define PIN_SDA D4 +#define PIN_SCL D5 -DigitalIn flight(D6); //フライトピン -DigitalOut SW(D7); -DigitalOut myled(LED1); //トリガー用 - - -int main() { +Serial pc(SERIAL_TX,SERIAL_RX,921600);//通信 +Serial xbee(D1,D0);//Xbeeのピン +DigitalOut FET(D9);//FETのピン +DigitalIn flight(D6); //フライトピンのピン +DigitalOut SW(D7);//フライトピンの電圧降下ピン +TB6612 motor(D7,D9,D11);//モータードライバーのピン +GPS gps (D1,D0); -SW=1; - flight==1;//flight pin ついてる - - myled=1; + + int main(){ + float x8; + FET=0; + SW=1; + flight==1;//フライトピンがついている + while(1) { if(flight==1) { - wait(10); - } + wait(1); + }//フライトピンがついているとき1秒待機 else{ if(flight==1) { - wait(10); + wait(1); } else{ - myled=0; + SW = 0; - printf("こんにちわ!"); //p23をlow(0V)にする。 + pc.printf("やったぞおおおおおおおおお!\n"); + break; - } - - - + } } } + BMP180 bmp180(PIN_SDA,PIN_SCL); + float pressure,temperature,altitude;//気圧,気温,高度 + int n; + pc.printf("\rstart!\n\r");//気圧センサースタート + bmp180.Initialize(27,BMP180_OSS_ULTRA_HIGH_RESOLUTION);//27は府大の海抜高度 + pc.printf("initialization complete!\n\r");//初期化完了 + + while(1){ + if(bmp180.ReadData(&temperature,&pressure)){ + float x4,x5,x6,x7,a,b; + a = pressure; + b = temperature; + x4 = 1019.11 / a; //海面気圧を気圧でわる + x5 = powf(x4, 0.1902225); //5.257ぶんの1 + x6 = 273.15 + b; //絶対温度 + x7 = (x5 - 1) * x6; + x8 = x7 / 0.0065; + altitude = x8; + + + pc.printf("Altitude(m)\t:%.3f\n\r",altitude); + pc.printf("--------------------------------\n\r"); + wait(3); + break; + n=0; + }else{ + pc.printf("NO DATA\n\r"); + pc.printf("---------------------------\n\r"); + wait(1); + } + } + while(1){ + if(bmp180.ReadData(&temperature,&pressure)){ + float y4,y5,y6,y7,y8,c,d; + float speed; + + c = pressure; + d = temperature; + y4 = 1019.11 / c; //海面気圧を気圧でわる + y5 = powf(y4,0.1902225); + y6 = 273.15 + d; + y7 = (y5 - 1) * y6; + y8 = y7 / 0.0065; + altitude = y8; + speed = (x8 - y8)/(float)(3+n);//値が取得でた場合は,3秒間の速さをだし,値が取得できなかった場合は3+n秒(nは値が取得できなかった回数)の速さをだす + + pc.printf("Altitude(m)\t:%.3f\n\r",altitude); + pc.printf("Speed(m/s)\t:%.3f\n\r",speed); + pc.printf("--------------------------------\n\r"); + x8 = y8; + n=0; + wait(3); + if(speed<=0){ + break; + } + }else{ + pc.printf("NO DATA\n\r"); + ++n; + wait(1); + } + } + /*speedが0以下になったらFETに20秒電流を流してその後電流を止める*/ +/* FET=1; +wait(20); +FET=0;*/ +motor = 100; +double a; + double b; + double distance; + + pc.printf("GPS begin\n"); + + while(1){ + if(gps.getgps()){ + /*a,bを緯度経度の初期値で初期化*/ + a=gps.latitude; + b=gps.longitude; + pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示 + pc.printf("--------------------------------\n\r"); + break; + }else{ + pc.printf("Fault_No_Data\r\n"); + wait(1); + } + } + while(1){ + if(gps.getgps()){ + pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示 + pc.printf("--------------------------------\n\r"); + + /*ここから距離の計算*/ + /*c、dを得た緯度経度の値で初期化*/ + double c; + double d; + c=gps.latitude; + d=gps.longitude; + + const double pi=3.14159265359;//円周率 + + /*ラジアンに変換*/ + double theta_a=a*pi/180; + double theta_b=b*pi/180; + double theta_c=c*pi/180; + double theta_d=d*pi/180; + + double e=sin(theta_a)*sin(theta_c)+cos(theta_a)*cos(theta_c)*cos(theta_b-theta_d);//2点間のなす角を求める + double theta_r=acos(e); + + const double earth_radius=6378140;//赤道半径 + + distance=earth_radius*theta_r;//距離の計算 + + /*距離が25m以上なら表示、通信*/ + if(distance>=30){ + pc.printf("run over 20m"); + motor=0; + pc.printf("run over 20m"); + break; + } + + }else { + pc.printf("False_No_Data\r\n"); + pc.printf("False_No_Date\r\n"); + wait(1); + }//データ取得失敗を表示、通信、1秒待機 + + } + pc.printf("成功\n"); + return 0; } \ No newline at end of file