File content as of revision 0:5ea8372bdf6e:

#include "mbed.h"
#include "math.h"
#include "BMP180.h"
#include "SDFileSystem.h"

#define  p0  1013.25f//海面気圧
#define  PI  3.1415
#define  Alt_num 3.0                                                            //何回の平均値にするか
#define  Angle_num 3                                                            //何回の平均値にするか
#define  Angle_cnt 10                                                           //何回姿勢角取得するか
#define  Alt_bou   30                                                           //この高度より低かったらFlight mode入ったとみなす

#define  Z_0    0.962                                                            //KXTC9-2050_rawを使ってキャリブレーションする
#define  Z_90   1.62
#define  Z_180  2.24

enum PHASE{CHECK, STANDBY, FLIGHT, EXPANSION, MISSION, WITHDRAW} Phase;

Serial          pc(USBTX, USBRX, 115200);                                                
Serial          twe(p28, p27, 115200);
DigitalOut      myled_1(LED1);
DigitalOut      myled_2(LED2);
DigitalOut      myled_3(LED3);
DigitalOut      myled_4(LED4);
DigitalIn       Flight_IN(p20);
SDFileSystem    sd(p5, p6, p7, p8, "sd");
BMP180          bmp(p9, p10);//(sda, scl)                                       
AnalogIn        z(p19);                                                         //加速度センサ
DigitalOut      load_sck(p14);                                                  //ロードセル 
DigitalIn       load_data(p15); 
DigitalOut      fet(p18);                                                       //ニクロム用MOSFET

PwmOut          servo(p24); 
PwmOut          motor1_1(p25);                                                  //motor1：回転用
PwmOut          motor1_2(p26);
DigitalOut      motor2_1(p22);                                                  //motor2：上昇下降用
DigitalOut      motor2_2(p23);
PwmOut          motor2_pwm(p21);

Timer           time_0;
Timer           time_flight;

int             _getTime();                                                     //time_0取得
float           _getAlt();                                                      //高度取得
float           _getAngle();                                                    //角度取得
int             _averageLoad(uint8_t times);                                    //ロードセルのキャリブレーションする時に使う
int             _getLoad();                                                                 
float           _getGram();                                                     //質量取得
void            _Rand_judge();                                                  //着地判定
void            _Angle_judge();

int             getTime;
float           getAlt, getAngle, getGram;;
float           Load_offset = 0;

FILE *fp;

int main(){
    
    /*タイマー*/
    time_0.start();
    
    /*SD
    fp = fopen("/sd/test.txt", "a");
    fprintf(fp, "Start.\r\n");
    fclose(fp);
    */
    /*BMP180*/
    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
    
    /*ロードセル
    load_sck = 1;                                                               
    wait_us(100);
    load_sck = 0;
    Load_offset = _averageLoad(10);
    */
    
    /*サーボ*/
    servo.period(0.020);   
    
    Phase = CHECK;
    
    switch(Phase){
        case CHECK: 
        
            //pc.printf("Check mode.\r\n");
            twe.printf("Check mode.\r\n");
            twe.printf("Check mode.\r\n");
            /*
            fp = fopen("/sd/test.txt", "a");
            fprintf(fp, "Check mode.\r\n");
            fclose(fp);
            */
            /*センサ取得*/
            for(int i=0; i<30; i++){
                        
                getTime = _getTime();
                getAlt = _getAlt();
                getAngle = _getAngle();
                getGram = _getGram();
                
                //pc.printf("Time: %d Alt: %f Angle: %f Gram: %f\r\n", getTime, getAlt, getAngle, getGram);
                twe.printf("Time: %d Alt: %f Angle: %f Gram: %f\r\n", getTime, getAlt, getAngle, getGram);
                wait(1.0);
                
            }
            /*
            fp = fopen("/sd/test.txt", "a");
            fprintf(fp, "Time: %d\r\n Alt: %f\r\n Angle: %f\r\n Gram: %f\r\n", getTime, getAlt, getAngle, getGram);
            fclose(fp);
            */
            myled_1 = 1;
            Phase = STANDBY;
            //break;
                    
        case STANDBY:
        
            //pc.printf("Standby mode.\r\n");
            twe.printf("Standby mode.\r\n");
            twe.printf("Standby mode.\r\n");
            /*
            fp = fopen("/sd/test.txt", "a");
            fprintf(fp, "Standby mode.\r\n");
            fclose(fp);
            */
            /*放出判定*/
            int fly_cnt = 0;
            while(1) {
                int FlightPin = Flight_IN;
                getAlt = _getAlt();
                //if((FlightPin == 0) && (getAlt < Alt_bou)){
                if(FlightPin == 0){
                    time_flight.start();
                    break;           
                }else{
                    fly_cnt++;
                    getTime = _getTime();
                    getAlt = _getAlt();
                    getAngle = _getAngle();
                    if(fly_cnt == 100){                                          //100回に1回データ保存
                        fly_cnt = 0;
                        /*
                        fp = fopen("/sd/test.txt", "a");
                        fprintf(fp, "Time: %d Alt: %f Angle: %f\r\n", getTime, getAlt, getAngle);
                        fclose(fp);
                        */
                    }                      
                }    
            }
            
            myled_2 = 1;
            Phase = FLIGHT;
            
        case FLIGHT:
            
            //pc.printf("Flight mode.\r\n");
            twe.printf("Flight mode.\r\n");
            twe.printf("Flight mode.\r\n");
            /*
            fp = fopen("/sd/test.txt", "a");
            fprintf(fp, "Flight mode.\r\n");
            fclose(fp);
            */
            /*着地判定*/
            _Rand_judge();
            
            //pc.printf("Rand judge success!!\r\n");
            twe.printf("Rand judge success!!\r\n");
            twe.printf("Rand judge success!!\r\n");
            
            myled_3 = 1;
            Phase = EXPANSION;
            
        case EXPANSION:
            
            //pc.printf("Expansion mode.\r\n");
            twe.printf("Expansion mode.\r\n");
            twe.printf("Expansion mode.\r\n");
            /*
            fp = fopen("/sd/test.txt", "a");
            fprintf(fp, "Expansion mode.\r\n");
            fclose(fp);
            */
            wait(30);
            
            /*ニクロム線動作*/
            fet = 1;
            wait(0.3);
            fet = 0;
            wait(2.0);
            fet = 1;
            wait(0.3);
            fet = 0;
            wait(30.0);
            
            
            for(int i=0; i<30; i++){
                
                getAngle = _getAngle();
                
                //pc.printf("Angle: %f\r\n", getAngle);
                twe.printf("Angle: %f\r\n", getAngle);
                /*
                fp = fopen("/sd/test.txt", "a");                                //毎回角度保存してるけどどうしようかな
                fprintf(fp, "Angle: %f\r\n", getAngle);
                fclose(fp);
                */
                wait(1.0);
                
            }
            
            myled_4 = 1;
            Phase = MISSION;
            
        case MISSION:
            
            //pc.printf("Mission mode.\r\n");
            twe.printf("Mission mode.\r\n");
            twe.printf("Mission mode.\r\n");
            /*
            fp = fopen("/sd/test.txt", "a");
            fprintf(fp, "Mission mode.\r\n");
            fclose(fp);
            */
            /*
            load_sck = 1;                                                               
            wait_us(100);
            load_sck = 0;
            Load_offset = _averageLoad(10);
            
            for(int i=0; i<10; i++){
                getGram += _getGram();
                wait(1.0);
            }
            
            Load_offset = getGram/10.0;
            
            
            getGram = 0.0;
            
            //pc.printf("Load offset: %f\r\n", Load_offset);
            twe.printf("Load offset: %f\r\n", Load_offset);
            
            fp = fopen("/sd/test.txt", "a");
            fprintf(fp, "Load offset: %f\r\n", Load_offset);
            fclose(fp);
            */
            
            /*サーボ*/
            servo.pulsewidth(0.0015);                                               //90度
            wait(1.0);
            servo.pulsewidth(0.0018);                                               //90度
            wait(1.0);
            servo.pulsewidth(0.0020);                                               //90度
            wait(1.0);
            servo.pulsewidth(0.0022);       
            wait(1.0);
            /*
            servo.pulsewidth(0.0024);
            wait(1.0);
            /*
            servo.pulsewidth(0.0028);
            wait(1.0);
            servo.pulsewidth(0.0030);
            */
            /*ドリル動作　motor1:回転、motor2:下降上昇*/
            motor2_pwm = 1.0;                                                   
            motor2_1 = 1;                                                           //下降・回転
            motor2_2 = 0;
            motor1_1 = 1.0;
            motor1_2 = 0.0;
            wait(9.0);
            motor2_1 = 0;                                                           //止まる・回転
            motor2_2 = 0;
            motor1_1 = 1.0;
            motor1_2 = 0.0;
            wait(2.0);
            motor1_1 = 0.0;                                                         //止まる・回転停止
            motor1_2 = 0.0;
            wait(3.0);
            motor2_1 = 0;                                                           //上昇・回転停止
            motor2_2 = 1;
            motor1_1 = 0.0;
            motor1_2 = 0.0;
            wait(13.0);
            motor2_1 = 0;                                                           //止まる・回転停止
            motor2_2 = 0;
            motor1_1 = 0.0;
            motor1_2 = 0.0;
            wait(2.0);
        
            /*サーボ動作*/
            servo.pulsewidth(0.0022);                                               //0度
            wait(1.0);
            servo.pulsewidth(0.0020);                                               //90度
            wait(1.0);
            servo.pulsewidth(0.0018);                                               //90度
            wait(1.0);
            servo.pulsewidth(0.0014);       
            wait(1.0);
            
            motor1_1 = 1.0;
            motor1_2 = 0.0;
            wait(10.0);
            motor1_1 = 0.0;
            motor1_2 = 0.0;
            
            
            /*質量値取得
            load_sck = 1;                                                               
            wait_us(100);
            load_sck = 0;
            Load_offset = _averageLoad(10);
            for(int i=0; i<10; i++){
                getGram = _getGram();
                //pc.printf("Gram: %f \r\n", getGram);
                twe.printf("Gram: %f \r\n", getGram);
                fp = fopen("/sd/test.txt", "a");                                    //最後の一回だけ保存してるけどどうしようかな
                fprintf(fp, "Gram: %f\r\n", getGram);
                fclose(fp);
                wait(1.0);
            }
            /*
            fp = fopen("/sd/test.txt", "a");                                    //最後の一回だけ保存してるけどどうしようかな
            fprintf(fp, "Gram: %f\r\n", getGram - Load_offset);
            fclose(fp);
            */
            Phase = WITHDRAW;
        
        case WITHDRAW:
        
            //pc.printf("Withdraw mode.\r\n");
            twe.printf("Withdraw mode.\r\n");
            twe.printf("Withdraw mode.\r\n");
            /*
            fp = fopen("/sd/test.txt", "a");
            fprintf(fp, "Withdraw mode.\r\n");
            fclose(fp);
            */
        
            while(1){
                
                myled_1 = !myled_1;
                myled_2 = !myled_2;
                myled_3 = !myled_3;
                myled_4 = !myled_4;
                
                getTime = _getTime();
                //pc.printf("Time: %d\r\n", getTime);
                twe.printf("Time: %d\r\n", getTime);
                
                wait(1.0);
            
            }
    }
}


int _getTime(){                                                                 //Timer_0取得
    int time;
    time = time_0.read();
    
    /*
    fp = fopen("/sd/test.txt", "a");
    fprintf(fp, "Time: %d\r\n",time);
    fclose(fp);
    */
    
    //pc.printf("%d\r\n", time);  
    return time;  
}


float _getAlt(){                                                                //高度取得
    float Tem, Pre, Alt, sum = 0;
    
    for(int i=0; i<Alt_num; i++){
        bmp.ReadData(&Tem, &Pre);
        Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
        sum += Alt;
    } 
    /*
    fp = fopen("/sd/test.txt", "a");
    fprintf(fp, "Alt: %f\r\n",sum/Alt_num);
    fclose(fp);
    */
    //pc.printf("%f, %f, %f\r\n",Tem, Pre, Alt);
    //pc.printf("Alt: %f\r\n", sum/Alt_num);  
    return sum/Alt_num;
}

float _getAngle(){                                                      
    float Z, rad, angle, Angle_sum = 0;
    for(int i=0; i<Angle_num; i++){
        Z = 3.3*z.read();
        if(Z <= Z_0){
            angle = 0;
        }else if(Z > Z_0 && Z < Z_90){
            rad = asin((Z - Z_0)/(Z_90 - Z_0));
            angle = 180*rad/PI;
        }else if(Z >= Z_90 && Z < Z_180){
            rad = asin((Z - Z_90)/(Z_180 - Z_90));
            angle = 90+180*rad/PI;
        }else{
            angle = 180;
        }
        /*
        if(Z <= Z_180){
            angle = 180;
        }else if(Z > Z_180 && Z < Z_90){
            rad = asin((Z - Z_180)/(Z_90 - Z_180));
            angle = 180 - 180*rad/PI;
        }else if(Z >= Z_90 && Z < Z_0){
            rad = asin((Z - Z_90)/(Z_0 - Z_90));
            angle = 90 - 180*rad/PI;                                      
        }else{
            angle = 0;                                                      //全部-180した
        }
        */
        Angle_sum += angle;
    }
    //pc.printf("Z:%f  ,%f °\r\n",  Z, Angle_sum/Angle_num); 
    return Angle_sum/Angle_num;
}

void _Rand_judge(){
    float Alt, Alt_old, Alt_new;
    float Alt_sum = 0;
    float Pre, Tem; 
    int rand_cnt = 0; 
    int flight_time;
    
    for(int i=0; i<Alt_num; i++){                                                     
        bmp.ReadData(&Tem, &Pre);
        Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
        Alt_sum += Alt;
    }
    Alt_old = Alt_sum/Alt_num;                                                      
    Alt_sum = 0;  

    while(1) {
        for(int j=0; j<Alt_num; j++){
            bmp.ReadData(&Tem, &Pre);
            Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
            Alt_sum += Alt;
        }
        Alt_new = Alt_sum/Alt_num;
        Alt_sum = 0;
        flight_time = time_flight.read();
        /*                                                                        //毎回データ保存してるけどどうしよ
        fp = fopen("/sd/test.txt", "a");
        fprintf(fp, "flight_time: %d , Alt_new: %f, rand_cnt: %d\r\n", flight_time, Alt_new, rand_cnt);
        fclose(fp);
        */
        if(fabsf(Alt_new-Alt_old)<0.8){                                         //何m以内の時着地判定クリアとするか  
            rand_cnt++;
            Alt_old = Alt_new;
            wait(0.97);                                                         //1Hzになるように調整
            if(rand_cnt == 3){                                                  //3回連続 または　60秒
                break;
            }         
        }else if(flight_time >= 60){
            break;
            
        }else{
            rand_cnt = 0;
            Alt_old = Alt_new;
            wait(0.97);
        }
        //pc.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt);
        twe.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt);
    } 
}

/*
void _Angle_judge(){
    int i;
    float Angle;
    int success_cnt = 0;
        Angle = _getAngle();
        Angle = _getAngle();                                                    //なぜか最初の二回は正しい値が出ない
    
    for(i=0; i<Angle_cnt; i++){
        Angle = _getAngle();
        pc.printf("Angle: %f°, cnt: %d\r\n", Angle, success_cnt);
        //twe.printf("Angle: %f°, cnt: %d\r\n", Angle, success_cnt);
            
        /*
        fp = fopen("/sd/test.txt", "a");
        fprintf(fp, "Angle: %f°, cnt: %d\r\n", Angle, success_cnt);
        fclose(fp);
        
        
        wait(1.0);
        if(Angle < 90){
            success_cnt++;
            if(success_cnt == 3){                                               //3回連続成功すれば
                    
                pc.printf("Angle judge success.");
                //twe.printf("Angle judge success.");
                /*
                fp = fopen("/sd/test.txt", "a");
                fprintf(fp, "Angle judge success.\r\n");
                fclose(fp);
                
                break;
            }
                    
        }else{
            success_cnt = 0;
            if((i+1) == Angle_cnt){
                pc.printf("Angle judge faild.");
                //twe.printf("Angle judge faild.");
                /*
                fp = fopen("/sd/test.txt", "a");
                fprintf(fp, "Angle judge failed.\r\n");
                fclose(fp);
                
                break;
            }    
        } 
    }
}
*/

int _averageLoad(uint8_t times){
    int sum = 0;
    for (int i = 0; i < times; i++){
        sum += _getLoad();
    }
    return sum / times;
}

int _getLoad(){
    int buffer; 
    buffer = 0 ;
    
    while (load_data.read()==1);
    
    for (uint8_t i = 24; i--;){
        load_sck=1;
        buffer = buffer << 1 ;
        if (load_data.read()){
            buffer ++;
        }
        load_sck=0;
    }
    
    for (int i = 0; i < 1; i++){
        load_sck=1; 
        load_sck=0;
    }
    
    buffer = buffer ^ 0x800000;
    return buffer;
}

float _getGram(){
    
    long val = (_averageLoad(2) - Load_offset);
    
    if(val < 0) val = 0;
    
    float scale = 0.0003*4.2987*128.0/100.0;                                    //定格出力：0.0006V，　　定格容量：100g
    float volt;
    float gram;
    volt = val*(4.2987/16777216.0);
    gram = volt/scale;
    
    return (float) gram;
}