File content as of revision 4:a44c7a8adaeb:

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

#define START_kasokudo 0
#define START_koudo 0
#define P_openkoudo 150
#define SETUP 0
#define LAUNCH 1
#define RISE 2
#define DROP1 3
#define DROP2 4
#define LAND 5

Degitalout 
Serial pc(USBTX,USBRX);
MS5607I2C ms(p9,p10,false);
MPU6050 mpu(p9,p10);
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);//後で削除//
PwmOut servo1(p23);
PwmOut servo2(p24);
PwmOut servo3(p25);
int Phase=SETUP;
Ticker tic;
float max=0;
int count_LAUNCH=0;
int count_RISE=0;
int count_DROP1=0;
Timer Time;
float LAUNCHtime,RISEtime,DROP1time;

void _judge();
float _mannaka

int main(){
    Timer start();
    Time.start();
    tic.attach(&_judge,0.1);
    while(1);
    return 0;
}

void _judge(){
     switch(Phase){
        case SETUP:
                Phase=LAUNCH;
                break;
                
        case LAUNCH: 
                myled1=0;
                float acc[3];
                mpu.getAccelero(acc);
                float acc_LAUNCH = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
                if(START_kasokudo < acc_LAUNCH){
                    count_LAUNCH++;
                    LAUNCHtime = Time.read();
                }
                if(Time.read() >= LAUNCHtime+0.5){
                    count_LAUNCH=0;
                }
                if(count_LAUNCH==5){
                    myled1=1;
                    Phase=RISE; 
                }
                break;
                
        case RISE:
                myled2=0;
                float alt_RISE = ms.getAltitude();
                if(max < alt_RISE+0.5){
                    max = alt_RISE;
                }else if(max+0.5 > alt_RISE){ 
                    count_RISE++;
                    RISEtime = Time.read();
                }
                if(Time.read() >= RISEtime + 1.0){
                    count_RISE = 0;
                }
                if(count_RISE==5){
                    servo1.pulsewidth(0.0015);
                    servo2.pulsewidth(0.0015);
                    myled2 = 1;
                    Phase=DROP1;
                }
                break;
                
        case DROP1:
                myled3=0;
                float alt_DROP1 = ms.getAltitude();
                if((alt_DROP1 - START_koudo) < P_openkoudo){
                    count_DROP1++;
                }
                if(Time.read() <= DROP1time + 0.5){
                    count_DROP1 = 0;
                }                                                                
                if(count_DROP1==5){
                    servo3.pulsewidth(0.0015);
                    wait(2.0);
                    myled3 = 1;
                    Phase=DROP2;
                }
                break;
                
        case DROP2:
                if(ms.getAltitude()== START_koudo){
                    //ここでサーボ電源切る卍卍//
                    Phase=LAND;
                }
                break;
                
                
        case LAND:
                break;
    }
}
    
float _mannaka{