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