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