narumi tatsuya
ロロロロ
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
--- /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
--- /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
--- /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
--- /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
--- /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
--- 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);//２点間のなす角を求める
+ 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