File content as of revision 1:68309ba0103d:

#include "mbed.h"
#include "math.h"
#include "MPU6050.h"
#include "MS5607I2C.h"

#define STANBY
#define LAUNCH
#define RISE
#define DROP
#define  ACC
#define SERCH_TIME
#define
#define
#define
#define


MPU6050 mpu(p9, p10);
MS5607I2C ms(p9, p10, false);
PwmOut servo1(p26);
PwmOut servo2(p25);
PwmOut servo3(p24);
DegitalOut myled1(LED1);
DegitalOut myled2(LED2);
DegitalOut myled3(LED3);
Timer timer;
Ticker ticker;


void _judge();
float _median(float[5]);


int main() {
    timer.start()
}

void _judge(){
    switch(FHASE){
        case STANBY:
                    break;
        case LAUNCH:
                    myled1 = 0;
                    float acc[3], acc_3;
                    int count = 0;
                    
                    getAccelero(acc);
                    acc_3 = pow(acc[0], 2) + pow(acc[1], 2) + pow(acc[2], 2);
                    acc_3 = sqrt(acc_3);
                    
                    if(acc >= ACC){
                        if(count == 0) SERCH_TIME = timer.read();
                        count++;
                    }
                    
                    if(timer.read() > time + SERCH_TIME){
                        count = 0;
                    }
                    
                    if(count == COUNT){
                        myled = 1;
                        PHASE = RISE;
                    }
                    
                    break;
        case RISE:
                    myled2 = 0;
                    
                    break;
        case DROP:
                    
                    break;
        
        }
}

float _median(){
    
}