main.cpp

00001 #include "mbed.h"
00002 #include "TB6612.h"
00003 #include "getGPS.h"
00004 #include "BMP180.h"
00005 #include <stdio.h>
00006 #include <math.h>
00007 
00008 #define PIN_SDA D4
00009 #define PIN_SCL D5
00010 
00011 Serial pc(SERIAL_TX,SERIAL_RX,921600);//通信
00012 Serial xbee(D1,D0);//Xbeeのピン
00013 DigitalOut FET(D9);//FETのピン
00014 DigitalIn flight(D6);  //フライトピンのピン    
00015 DigitalOut SW(D7);//フライトピンの電圧降下ピン 
00016 TB6612 motor(D7,D9,D11);//モータードライバーのピン
00017 GPS gps (D1,D0);                 
00018 
00019 
00020   int main(){
00021     float x8;
00022     FET=0;
00023     SW=1;
00024     flight==1;//フライトピンがついている
00025 
00026    
00027      while(1) {
00028     if(flight==1) {
00029         wait(1);
00030         }//フライトピンがついているとき1秒待機
00031         
00032     else{
00033         if(flight==1) {
00034         wait(1);
00035         }
00036         else{
00037 
00038         SW = 0;
00039         pc.printf("やったぞおおおおおおおおお!\n");
00040   
00041     break;
00042             } 
00043         }
00044    }
00045     BMP180 bmp180(PIN_SDA,PIN_SCL);
00046     float pressure,temperature,altitude;//気圧，気温，高度
00047     int n;
00048     pc.printf("\rstart!\n\r");//気圧センサースタート
00049     bmp180.Initialize(27,BMP180_OSS_ULTRA_HIGH_RESOLUTION);//27は府大の海抜高度
00050     pc.printf("initialization complete!\n\r");//初期化完了
00051 
00052     while(1){
00053         if(bmp180.ReadData(&temperature,&pressure)){
00054           float x4,x5,x6,x7,a,b;
00055             a = pressure;
00056             b = temperature;
00057             x4 = 1019.11 / a; //海面気圧を気圧でわる
00058             x5 = powf(x4, 0.1902225); //5.257ぶんの1
00059             x6 = 273.15 + b; //絶対温度
00060             x7 = (x5 - 1) * x6;
00061             x8 = x7 / 0.0065;
00062             altitude = x8;
00063             
00064             
00065             pc.printf("Altitude(m)\t:%.3f\n\r",altitude);
00066             pc.printf("--------------------------------\n\r");
00067             wait(3);
00068             break;
00069             n=0;
00070     }else{
00071         pc.printf("NO DATA\n\r");
00072         pc.printf("---------------------------\n\r");
00073         wait(1);
00074         }
00075     }
00076         while(1){
00077             if(bmp180.ReadData(&temperature,&pressure)){
00078                 float y4,y5,y6,y7,y8,c,d;
00079                 float speed;
00080                 
00081                 c = pressure;
00082                 d = temperature;
00083                 y4 = 1019.11 / c; //海面気圧を気圧でわる
00084                 y5 = powf(y4,0.1902225);
00085                 y6 = 273.15 + d;
00086                 y7 = (y5 - 1) * y6;
00087                 y8 = y7 / 0.0065;
00088                 altitude = y8;
00089                 speed = (x8 - y8)/(float)(3+n);//値が取得でた場合は，3秒間の速さをだし，値が取得できなかった場合は3＋n秒(nは値が取得できなかった回数)の速さをだす
00090                 
00091                 pc.printf("Altitude(m)\t:%.3f\n\r",altitude);
00092                 pc.printf("Speed(m/s)\t:%.3f\n\r",speed);
00093                 pc.printf("--------------------------------\n\r");
00094                 x8 = y8;
00095                 n=0;
00096                 wait(3);
00097                 if(speed<=0){
00098                     break;
00099                     }
00100             }else{
00101                     pc.printf("NO DATA\n\r");
00102                     ++n;
00103                     wait(1);
00104                     }
00105            }
00106            /*speedが0以下になったらFETに20秒電流を流してその後電流を止める*/         
00107 /* FET=1;
00108 wait(20);
00109 FET=0;*/
00110 motor = 100; 
00111 double a;
00112     double b;
00113     double distance;
00114     
00115      pc.printf("GPS begin\n");
00116     
00117     while(1){
00118         if(gps.getgps()){
00119             /*a,bを緯度経度の初期値で初期化*/
00120             a=gps.latitude;
00121             b=gps.longitude;
00122             pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
00123             pc.printf("--------------------------------\n\r");
00124                       break;
00125         }else{
00126             pc.printf("Fault_No_Data\r\n");
00127          wait(1);
00128         }
00129         }  
00130          while(1){
00131          if(gps.getgps()){
00132              pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示
00133             pc.printf("--------------------------------\n\r");
00134              
00135     /*ここから距離の計算*/
00136                  /*c、dを得た緯度経度の値で初期化*/  
00137                      double c;
00138                       double d;
00139                       c=gps.latitude;
00140                       d=gps.longitude;
00141                       
00142                       const double pi=3.14159265359;//円周率
00143                       
00144                       /*ラジアンに変換*/
00145                       double theta_a=a*pi/180;
00146                       double theta_b=b*pi/180;
00147                       double theta_c=c*pi/180;
00148                       double theta_d=d*pi/180;
00149                       
00150                       double e=sin(theta_a)*sin(theta_c)+cos(theta_a)*cos(theta_c)*cos(theta_b-theta_d);//２点間のなす角を求める
00151                       double theta_r=acos(e);
00152                       
00153                       const double earth_radius=6378140;//赤道半径
00154                       
00155                       distance=earth_radius*theta_r;//距離の計算
00156                       
00157              /*距離が25m以上なら表示、通信*/         
00158                  if(distance>=30){
00159                   pc.printf("run over 20m");
00160                   motor=0;
00161                   pc.printf("run over 20m");
00162                   break;
00163                   }
00164 
00165             }else {
00166                 pc.printf("False_No_Data\r\n");
00167                 pc.printf("False_No_Date\r\n");
00168                 wait(1);
00169             }//データ取得失敗を表示、通信、1秒待機
00170                
00171                }
00172     pc.printf("成功\n");
00173    return 0;
00174    }