File content as of revision 10:c96e83e35ed2:

#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

Serial pc(SERIAL_TX,SERIAL_RX,921600);//通信
BMP180 bmp180(PIN_SDA,PIN_SCL);//気圧センサーのピン
Serial xbee(D1,D0);//Xbeeのピン
DigitalOut FET1(D9);//FETのピン
DigitalOut FET2(D8);
DigitalIn flight(D6);  //フライトピンのピン    
DigitalOut SW(D7); 
TB6612 motor(D7,D9,D11);//モータードライバーのピン
GPS gps (D13,A7);                 


  int main(){
    float avalt,x8;
    float pressure,temperature,altitude;//気圧，気温，高度
    FET1=0;
    FET2=1;
 wait(10);
    FET2=0;
    FET1=1;
    wait(10);
    FET1=0;
    FET2=1;
    wait(10);
    
    SW=1;
    flight==1;//フライトピンがついている
    
while(1) {
    if(flight==1) {
        wait(1);
        }//フライトピンがついているとき1秒待機
        
    else{
        if(flight==1) {
        wait(1);
        }
        else{

        SW = 0;
        FET2=1;
        wait(25);
        xbee.printf("やったぞおおおおおおおおお!\n");
  
    break;
            } 
        }
   }
    
    float avalt,x8;
    float pressure,temperature,altitude;//気圧，気温，高度
    int t=0;

    xbee.printf("\rstart!\n\r");//気圧センサースタート
    bmp180.Initialize(27,BMP180_OSS_ULTRA_HIGH_RESOLUTION);//27は府大の海抜高度
    xbee.printf("initialization complete!\n\r");//初期化完了

 while(1){
          float x4,x5,x6,x7,a,b;
          float sum=0,altitude[11];
          int r=0;
        for(int p=1;p<11;p++){
        if(bmp180.ReadData(&temperature,&pressure)){
            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[p] = x8;
            sum=sum+altitude[p];
            r++;
            xbee.printf("altitude(m)\t:%.3f\n\r",altitude[p]);
            xbee.printf("--------------------------------\n\r");
        }else{
        xbee.printf("NO DATA\n\r");
        xbee.printf("---------------------------\n\r");
        wait(1);
            }
            }
            avalt=sum/(float)r;
            xbee.printf("Altitude(m)\t:%.3f\n\r",avalt);
            xbee.printf("--------------------------------\n\r");
            wait(3);
            break;
        }
        while(1){
                float y4,y5,y6,y7,c,d;
                float y8=avalt;
                float speed;
                float s=0;
                float alti[128];
                float average_alti;
                int e=0;
                int i=1;
            
        for(int q=1;q<11;q++){
                if(bmp180.ReadData(&temperature,&pressure)){
                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;
                alti[q]= y8;
                 s=s+alti[q];
                xbee.printf("高度%d回目%f\n\r",q-i,alti[q]);
                xbee.printf("-------------------------------\n\r");
                e++;
                }else{
                xbee.printf("NO_Data\n\r");
                    }
                 }   
                average_alti=s/(float)e;
                speed = (avalt - average_alti)/3;
                
                xbee.printf("Altitude(m)\t:%.3f\n\r",average_alti);
                xbee.printf("Speed(m/s)\t:%.3f\n\r",speed);
                xbee.printf("-------------------------------\n\r");
                
                avalt=average_alti;
                xbee.printf("tの値は%d\n",t);
                i++;
                if(speed<=0.5){
                    t=t+1;
                    }if(t==3){
                        break;
                        }
            wait(3);
            }
            }


           /*speedが3回0.5m/s以下になったらFETに20秒電流を流してその後電流を止める*/         
        FET1=1;
        wait(10);
        FET1=0;
        motor = 100; 
    double a;
    double b;
    double distance;
    
     xbee.printf("GPS begin\n");
    
    while(1){
        if(gps.getgps()){
            /*a,bを緯度経度の初期値で初期化*/
            a=gps.latitude;
            b=gps.longitude;
            xbee.printf("(%lf,%lf)\r\n",a,b);//緯度と経度を表示
            xbee.printf("--------------------------------\n\r");
                      break;
        }else{
            xbee.printf("Fault_No_Data\r\n");
         wait(1);
        }
        }  
         while(1){
         if(gps.getgps()){
             xbee.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示
            xbee.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);//２点間のなす角を求める
                      double theta_r=acos(e);
                      
                      const double earth_radius=6378140;//赤道半径
                      
                      distance=earth_radius*theta_r;//距離の計算
                      
             /*距離が30m以上なら表示、通信*/         
                 if(distance>=30){
                  pc.printf("run over 20m");
                  motor=0;
                  xbee.printf("run over 20m");
                  break;
                  }

            }else {
                xbee.printf("False_No_Data\r\n");
                pc.printf("False_No_Date\r\n");
                wait(1);
            }//データ取得失敗を表示、通信、1秒待機
               
               }
    xbee.printf("成功\n");
   return 0;
   }