Revision 2:98629c0bb9ff, committed 2020-12-17

Comitter:

naruu

Date:

Thu Dec 17 17:29:28 2020 +0000

Parent:

1:7382df606336

Child:

3:258c138c6299

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