ロロロロ

Dependencies:   mbed BMP180

Files at this revision

API Documentation at this revision

Comitter:
naruu
Date:
Thu Dec 17 17:29:28 2020 +0000
Parent:
1:7382df606336
Commit message:
no;

Changed in this revision

BMP180.lib Show annotated file Show diff for this revision Revisions of this file
TB6612.cpp Show annotated file Show diff for this revision Revisions of this file
TB6612.h Show annotated file Show diff for this revision Revisions of this file
getGPS.cpp Show annotated file Show diff for this revision Revisions of this file
getGPS.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
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