File content as of revision 0:266e722e382f:

#include "mbed.h"
//#include "MS5607SPI.h"
#include "MS5607I2C.h"
#include "MPU6050.h"
#include "SDFileSystem.h"
#define ON  1
#define OFF 0
#define acc 16384
#define TBD1 0.95             //TBDは適当に設定
#define TBD2 10
#define TBD3 0.98
#define TBD4 15
#define TBD5 5
#define TBD6 20
#define TBD7 8

//MS5607SPI ms5607(p11, p12, p13, p3);
MS5607I2C ms5607(p9,p10, false);
MPU6050 mpu(p9,p10);
SDFileSystem sd(p5,p6,p7,p8,"sd");
PwmOut servo1(p21);           //パラシュート開放用servo
PwmOut servo2(p22);
PwmOut servo3(p23);           //CanSat開放用servo
PwmOut servo4(p24);        
Serial pc(USBTX, USBRX);
FILE*fp;
Timer timer1,timer2,timer3;
Ticker ticker;
double decision1,decision2,decision3;
double time1,time2,time3;

float a[3];
float g[3];
float Pre=ms5607.getPressure();
float Tem=ms5607.getTemperature();
float Alt = ms5607.getAltitude();

void dateWriting()
{
    fp=fopen("Fright_date.txt","a");

    mpu.getAccelero(a);
    mpu.getGyro(g);

    fprintf(fp,"%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",a[0],a[1],a[2],g[0],g[1],g[2],Pre,Tem,Alt);

    fclose(fp);
}

int main()
{
    fp=fopen("Fright_date.txt","a");

    if(fp==NULL) {
        pc.printf("Open Failed!\n");
    } else {
        pc.printf("Open Success!\n");
    }

    pc.printf("Start!\n");

    fprintf(fp,"加速度x,加速度y,加速度z,ジャイロx,ジャイロy,ジャイロz,気圧[Pa],温度[degC],高度[m]\r\n");

    servo1.period_ms(20);
    servo2.period_ms(20);                   //パルス周期＝20ms
    servo3.period_ms(20);
    servo4.period_ms(20);

    //SG90の動作パルス幅は50µs~2400µs(0度~180度)
    servo1.pulsewidth(0.0005);
    servo2.pulsewidth(0.0005);
    servo3.pulsewidth(0.0005);
    servo4.pulsewidth(0.0005);

    while(decision1<0.9) {                  //取得加速度の9割がTBD3より大きければループを抜ける
        mpu.setAcceleroRange(0);            //変数初期化
        mpu.setGyroRange(0);

        while(a[2]<TBD1) {
            ticker.attach(&dateWriting, 0.1);
        }

        /*************************発射判定ループ開始*************************/

        while(time1<TBD2) {
            int i,counter1=0;
            float az[i];                                  //取得加速度を格納する配列要素
            for(i=0; i<=20; i++) {                        //条件式はTBD2により決定
                timer1.start();
                ticker.attach(&dateWriting, 0.1);         //タイマー割込み
                az[i]=a[2];                     
                if(az[i]>TBD3) {
                    counter1++;                           //取得加速度がTBD3より大きければプラス1
                }
                timer1.stop();
                time1=timer1.read();
            }
            decision1=counter1/(i+1);                     //(TBD3より大きい取得加速度の数)÷(取得加速度の総数)
        }
    }

    while(decision2<0.9) {                                //取得高度の9割が下降を検知すればループを抜ける
        int j=0;
        float alt1[j];                                    //取得高度を各格納する配列要素
        
        while(j>0&&alt1[j]>alt1[j-1]) {
            ticker.attach(&dateWriting, 0.1);             //タイマー割込み
            alt1[j]=Alt;      
            j++; 
        }

        /*************************頂点判定ループ開始*************************/

        while(time2<TBD4) {
            int k,counter2=0;
            float alt2[k];                                //取得高度を格納する配列要素
            for(k=0; k<=20; k++) {                        //条件式はTBD4により決定
                timer2.start();
                ticker.attach(&dateWriting, 0.1);         //タイマー割込み
                alt2[k]=Alt;
                if(k>0&&alt2[k]<alt2[k-1]) {
                    counter2++;                           //下降検知をすればプラス1
                }
                timer2.stop();
                time2=timer2.read();
            }
            decision2=counter2/(k+1);                     //(下降検知した回数)÷(取得高度の総数)
        }
    }

    wait(0.1);
    servo1.pulsewidth(0.0015);                            //CanSat開放用servo                  　　
    servo2.pulsewidth(0.0015);
    
    while(decision3<0.6) {                                //取得高度の6割が初期高度+TBD5以下ならばループを抜ける
        int m=0;
        float alt3[m];                                    //取得高度を格納する配列要素
        float datum1=alt3[m]+TBD5;
        
        while(alt3[0]>datum1) {
            ticker.attach(&dateWriting, 0.1);             //タイマー割込み
            alt3[m]=Alt;
            m++;
        }

        /*************************高度判定ループ開始*************************/

        while(time3<TBD6) {
            int n,counter3=0;
            float alt4[n],datum2=alt4[n]+TBD7;
            for(n=0; n<=20; n++) {                        //条件式はTBD6により決定
                timer3.start();
                ticker.attach(&dateWriting, 0.1);         //タイマー割込み
                alt4[n]=Alt;
                if(alt4[n]<=datum2) {
                    counter3++;                           //取得高度の6割が初期高度+TBD5以下ならばプラス1
                }
                timer3.stop();
                time3=timer3.read();
            }
            decision3=counter3/(n+1);                     //(取得高度のうち初期高度+TBD5以下の数)÷(取得高度の総数)
        }
    }

    wait(0.1);
    servo3.pulsewidth(0.0015);                            //CanSat開放(servo角度を90度に設定)
    servo4.pulsewidth(0.0015);

    pc.printf("Finish!\n");
}