Diff: main.cpp

Revision:

1:1dab888e1f3c

Parent:

0:5cddbcb7193f

Child:

2:20304b3cea67

--- a/main.cpp	Tue Dec 15 08:36:33 2020 +0000
+++ b/main.cpp	Wed Dec 16 07:18:54 2020 +0000
@@ -1,7 +1,169 @@
 #include "mbed.h"
 #include "TB6612.h"
-TB6612 motor(D7,D9,D11);
-int main(){
-  motor=100; 
+#include "getGPS.h"
+#include"math.h"
+#include "BMP180.h"
+#include <stdio.h>
+
+#define PIN_SDA D4
+#define PIN_SCL D5
+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);//モータードライバーのピン
+
+
+  int main(){
+    float x8;
+    FET=0;
+    SW=1;
+    flight==1;//フライトピンがついている
+
+   
+     while(1) {
+    if(flight==1) {
+        wait(1);
+        }//フライトピンがついているとき1秒待機
+        
+    else{
+        if(flight==1) {
+        wait(1);
+        }
+        else{
+
+        SW = 0;
+        xbee.printf("やったぞおおおおおおおおお!\n");
+        pc.printf("やったぞおおおおおおおおお!\n");//フライトピンが外れたときに表示  
+    break;
+            } 
+        }
+   }
+    BMP180 bmp180(PIN_SDA,PIN_SCL);
+    float pressure,temperature,altitude;//気圧，気温，高度
+    xbee.printf("\rstart!\n\r");//気圧センサースタート
+    bmp180.Initialize(27,BMP180_OSS_ULTRA_HIGH_RESOLUTION);//27は府大の海抜高度
+    xbee.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;
+            
+            
+            xbee.printf("Altitude(m)\t:%.3f\n\r",altitude);
+            xbee.printf("--------------------------------\n\r");
+            wait(1);
+            break;
+    }else{
+        xbee.printf("NO DATA\n\r");
+        xbee.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)/1;
+                
+                xbee.printf("Altitude(m)\t:%.3f\n\r",altitude);
+                xbee.printf("Speed(m/s)\t:%.3f\n\r",speed);
+                xbee.printf("--------------------------------\n\r");
+                x8 = y8;
+                wait(1);
+                if(speed<=0){
+                    break;
+                    }}
+                else{
+                    xbee.printf("NO DATA\n\r");
+                    wait(1);
+                    }
+           }
+           /*speedが0以下になったらFETに20秒電流を流してその後電流を止まる*/         
+    FET=1;
+wait(20);
+FET=0;
+
+}
+  //motor=100; 
+  
+   double a;
+    double b;
+    double distance;
+    
+     xbee.printf("GPS begin\n");;
+    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);//緯度と経度を表示
+                      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);//緯度と経度を表示
+             
+    /*ここから距離の計算*/
+                 /*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;//距離の計算
+                      
+             /*距離が20m以上なら表示、通信*/         
+                 if(distance>=25){
+                  pc.printf("run over20m");
+                  motor=0;
+                  xbee.printf("run over20m");
+                  break;
+                  }
+
+            }else {
+                xbee.printf("False_No_Data\r\n");
+                pc.printf("False_No_Date\r\n");
+                wait(1);
+            }//データ取得失敗を表示、通信、1秒待機
+               
+               }
+    
    return 0;
    }
\ No newline at end of file