File content as of revision 0:a38bf4917128:

#include "mbed.h"
#include "math.h"
#include "string.h"
#include "GPS_interrupt.h"
#include "LPS25H_spi.h"
#include "Motor_lib.h"
#include "Nichrome_lib.h"
#include "ADXL375_spi.h"

/*
キャンパスコモン
Lat : 33.594886
Lon : 130.217847

能代
Lat : 40.14237977
Lon : 139.9872893

種コン：
Lat : 30.37451499
Lon : 130.95993809
*/

double lat_target = 30.37451499;
double lon_target = 130.95993809;

bool waitGPS = true;
bool flag_RPi_setting = true;

char file_name[32] = "/local/stable2.csv";

bool cv_yel = true;

/************************************************
コンストラクタ
************************************************/
Serial pc(USBTX, USBRX, 115200);
Serial GPS_serial(p9, p10, 38400);
Serial XB(p28, p27, 115200);
Serial RPi(p13, p14, 115200);
SPI wspi(p5, p6, p7);
LocalFileSystem local("local");
FILE *fp; //ローカルファイル

LPS25H_spi LPS25H(wspi, p11);
ADXL375_spi ADXL375(wspi, p8);
GPS_interrupt GPS(&GPS_serial);
Nichrome_lib Nichrome1(p20);
Nichrome_lib Nichrome2(p19);
Motor_lib MotorR(p24, p23);
Motor_lib MotorL(p21, p22);

AnalogIn Vinpin(p15);
InterruptIn GPSAve(p17);
InterruptIn Button(p16);
InterruptIn FlightPin(p18);

Ticker tick_sensor;
Ticker tick_GPS;
Ticker sendTick_XB;
Ticker sendTick_SD;
Ticker sendTick_RPi;

Timer timer;

/************************************************
関数のプロトタイプ宣言
************************************************/
void setup(); //セットアップ

void phase1(); //準備～キャリア待機
void phase2(); //降下中
void phase3(); //分離・展開
void phase4(); //走行（GPS）
void phase5(); //走行（カメラ）
void phase6(); //回収待機
void flight(); //割り込み：フライトモード移行

int setRTG();    //ゴール方向を計算
float setLTG();  //ゴールまでの距離を計算
void escape();   //スタック脱出
void escape2();
void activate(); //旋回

void readGPS();    //GPSの座標読み取り
void readSensor(); //センサーの値の取得
void resetPT();    //割り込み：0m地点での気圧・温度のリセット

void readRPi();     //RPiからのデータ読み取り
void sendRPi();     //RPiにデータ送信
void readCommand(); //RPiコマンド解析

void ConnectCheck(); //割り込み：アクチュエータチェック
void startGPSAve();  //割り込み：GPS平均計算開始
void stopGPSAve();   //割り込み：GPS平均計算終了
void GPSAverage();   //GPS平均計算
void stopInterruptIn(); //割り込み：ピン割り込み停止
void buttonPush();   //割り込み：ボタンを押したとき
void buttonRelease();//割り込み：ボタンを離したとき

void printData(); //PCでデータ表示
void sendData();  //地上局へデータ送信
void recData();   //ログ保存
void startRec();  //記録開始
void stopRec();   //記録終了

/************************************************
変数の宣言
************************************************/
double gps_lat, gps_lon, lat_0, lon_0, gps_alt, gps_knot, gps_deg;
double gps_lat_raw[4], gps_lon_raw[4];
int raw_count = 0;
float utc[6];
int gps_sat;
bool flag_ave = false;
double lat_sum, lon_sum, lat_ave, lon_ave, csum;

float pres, temp, alt;
float pres_0, temp_0;
int sep_count = 0;

float acc[3], gravity;

float vin;

int CanSat = 1; //CanSat mode
int w_time;     //wait timer
int rtg;        //rad to goal
double ltg;     //length to goal
int reset_count;

int f_time = 20; //分離タイマー

bool esc = false;
bool save = false;
bool flight_yet = false;
bool safety = false;
bool nichrome1_yet = false;
bool interrupt = false; //割り込み処理中
bool button_push = false;
bool phase4_once = false;

char RPi_read[128];
int RPi_pt = 0; //RPiコマンド文字列のポインタ
int red_per, red_center_x, red_center_y, red_moment_x, red_moment_y, yel_per; //画像認識のデータ
int laser; //距離センサのデータ

/************************************************
メイン関数
************************************************/
int main() {
    setup();
    
    RPi.attach(&readRPi, Serial::RxIrq);
    
    tick_GPS.attach(&readGPS, 0.25f);
    tick_sensor.attach(&readSensor, 0.05f);
    
    phase1(); //準備～キャリア待機
    
    phase2(); //降下中
    
    phase3(); //分離・展開
    
    phase4(); //走行（GPS）
    
    phase5(); //走行（カメラ）
    
    phase6(); //回収待機
}

/************************************************
Phase1
準備～キャリア待機中
************************************************/
void phase1(){
    sendData();
    recData();
    sendRPi();
    sendTick_XB.attach(&sendData, 1.0f);
    sendTick_SD.attach(&recData, 1.0f);
    sendTick_RPi.attach(&sendRPi, 2.0f);
    
    FlightPin.mode(PullUp);
    FlightPin.rise(&flight);
    
    Button.mode(PullUp);
    Button.fall(&buttonPush);
    Button.rise(&buttonRelease);
    
    GPSAve.mode(PullUp);
    GPSAve.fall(&startGPSAve);
    GPSAve.rise(&stopGPSAve);
    
    w_time = 0;
    
    while(CanSat == 1){ 
        pc.printf("");
    }
}

/************************************************
Phase2
降下中
************************************************/
void phase2(){
    FlightPin.fall(&stopInterruptIn);
    Button.fall(&stopInterruptIn);
    Button.rise(&stopInterruptIn);
    GPSAve.fall(&stopInterruptIn);
    GPSAve.rise(&stopInterruptIn);
    
    sendTick_XB.detach();
    sendTick_SD.detach();
    sendData();
    recData();
    sendTick_XB.attach(&sendData, 0.25f);
    sendTick_SD.attach(&recData, 0.1f);
    
    //Timer timer;
    timer.reset();
    timer.start();
    //int count_down = 0;
    
    sendRPi();
    
    while(CanSat == 2){
        w_time = f_time - timer.read();
        
        //フライト後すぐにスタビ展開///////////////////////////////////////////////
        if(!nichrome1_yet){
                Nichrome1.fire(5.0f);
                //pc.printf("Separate 1\r\n");
                nichrome1_yet = true;
        }        
        //ここまで//////////////////////////////////////////////////////////////
        
        if(sep_count > 10 || w_time == 0){
            if(!Nichrome1.status){
                CanSat = 3;
                timer.stop();
                timer.reset();
                w_time = 0;
                }
        }
    }
}

/************************************************
Phase3
分離・展開
************************************************/
void phase3(){
    while(CanSat == 3){
        if(!Nichrome1.status){
            Nichrome2.fire(5.0f);
            //pc.printf("Separate 2\r\n");
            timer.reset();
            timer.start();
            do{
                w_time = 6 - timer.read();
            }while(w_time > 0);
            timer.stop();
            timer.reset();
            w_time = 0;
            CanSat = 4;
        }
    }
}

/************************************************
Phase4
GPS走行中
************************************************/
void phase4(){
    sendTick_XB.detach();
    sendTick_SD.detach();
    sendData();
    recData();
    sendTick_XB.attach(&sendData, 1.0f);
    sendTick_SD.attach(&recData, 1.0f);
    
    if(!phase4_once){
        phase4_once = true;
        //パラが引っかからないようにスタート
        for(int i = 0; i < 10; i++){
            MotorL.turn_a(0.8f);
            MotorR.turn_a(0.8f);
            wait(0.1f);
            MotorL.stop();
            MotorR.stop();
            wait(0.3f);
        }
        //ここまで
        
        //30s待機
        w_time = 30;
        timer.reset();
        timer.start();
        while(w_time > 0){
            w_time = 30 - timer.read();
        }
    }
    
    //移動開始
    MotorL.turn_a(0.8f);
    MotorR.turn_a(1.0f);
    timer.reset();
    timer.start();
    w_time = 3.0f;
    while(w_time >= 0){
        w_time = 3.0 - timer.read();
    }
    
    while(CanSat == 4){
        if(reset_count >= 5){
            reset_count = 0;
            if((abs(gps_lat - lat_0) <= 0.000005 && abs(gps_lon - lon_0) <= 0.000005) || gps_knot < 0.3f){
                escape();
            }
            activate();
            
            lat_0 = gps_lat;
            lon_0 = gps_lon;
        }
        
        if(yel_per > 20 && cv_yel){
            escape2();
            MotorL.turn_a(0.8f);
            MotorR.turn_a(1.0f);
        }
        
        if(abs(gps_lat - lat_target) < 0.00004 && abs(gps_lon - lon_target) < 0.00004){
            CanSat = 5;
            break;
        }
        
        wait(0.1f);
    }
}

/************************************************
Phase5
画像認識走行中
************************************************/
void phase5(){
    sendTick_RPi.detach();
    sendRPi();
    sendTick_XB.attach(&sendData, 1.0f);
    sendTick_SD.attach(&recData, 1.0f);
    sendTick_RPi.attach(&sendRPi, 0.25f);
    
    int w_time_camera = 180;
    timer.reset();
    timer.start();
    
    while(CanSat == 5){
        
        w_time = w_time_camera - timer.read();
        if(w_time <= 0){
            CanSat = 6;
        }
        
        ////////////////////////////////////////////////////////////////////////天気を見て変える
        if(red_per >= 1){
            MotorL.turn_a(0.6f);
            MotorR.turn_a(0.8f);
            if(red_per >= 40){
                CanSat = 6;
            }
            else if(red_per < 3){
                wait(0.5f);
            }
            else if(red_per < 8){
                wait(1.0f);
            }
            else if(red_per < 13){
                wait(1.5f);
            }
            else{
                wait(2.0f);
            }
        }
        else{
            MotorL.turn_a(0.3f);
            MotorR.turn_a(0.6f);
        }
        
        /*
        if((red_per >= 5 && red_center_x != -1) || red_per >= 10){
            if(red_per >= 40){      //天候次第
                CanSat = 6;
            }
            */
            /*
            MotorL.turn_a(0.4f);    //グラウンド状況次第
            MotorR.turn_a(0.2f);
            wait(0.5f);
            */
            /*
            MotorL.turn_a(0.6f);
            MotorR.turn_a(0.8f);
            wait(0.5f);
        }
        else{
            MotorL.turn_a(0.3f);
            MotorR.turn_a(0.6f);
        }
        */
    }
    MotorL.stop();
    MotorR.stop();
}

/************************************************
Phase6
終了・回収待機中
************************************************/
void phase6(){
    sendTick_XB.detach();
    sendTick_SD.detach();
    sendTick_RPi.detach();
    sendData();
    recData();
    sendRPi();
    sendTick_XB.attach(&sendData, 5.0f);
    sendTick_SD.attach(&recData, 5.0f);
    sendTick_RPi.attach(&sendRPi, 1.0f);
    
    Button.fall(&buttonPush);
    
    while(CanSat == 6){
    }
}

/************************************************
割り込み：Phase1 -> Phase2
************************************************/
void flight(){
    if(!interrupt && !flight_yet && safety){
        CanSat = 2;
        flight_yet = true;
    }
}

/************************************************
ゴール方向を計算
************************************************/
int setRTG(){
    double target_rad0 = atan2(lat_target - gps_lat, lon_target - gps_lon) - atan2(gps_lat - lat_0, gps_lon - lon_0);
    double target_rad1 = target_rad0 * 180 / atan(1.0) / 4;
    int target_rad = (int)target_rad1;
    if(target_rad > 180){
        return target_rad - 360;
    }
    else if(target_rad < -180){
        return target_rad + 360;
    }
    else{
        return target_rad;
    }
}

/************************************************
ゴールまでの距離を計算
************************************************/
float setLTG(){
    float r_earth = 40 * pow(10.0f, 6);
    float LTG_lat = r_earth / 360 * (lat_target - gps_lat);
    float LTG_lon = r_earth * cos(lat_target / 180 * 3.141592654) / 360 * (lon_target - gps_lon);
    
    return sqrt(pow(LTG_lat, 2) + pow(LTG_lon, 2));
}

/************************************************
スタック脱出
************************************************/
void escape(){
    sendTick_XB.detach();
    sendTick_SD.detach();
    sendTick_RPi.detach();
    sendData();
    recData();
    sendRPi();
    sendTick_XB.attach(&sendData, 1.0f);
    sendTick_SD.attach(&recData, 1.0f);
    sendTick_RPi.attach(&sendRPi, 1.0f);
    
    Timer timer;
    
    esc = true;
    MotorL.break_stop();
    MotorR.break_stop();
    wait(0.5f);
    
    timer.start();
    MotorL.turn_b();
    MotorR.turn_b();
    do{
        w_time = 4 - timer.read();
    }while(w_time > 2);
    
    MotorL.stop();
    MotorR.turn_b();
    do{
        w_time = 4 - timer.read();
    }while(w_time > 1);
    
    MotorL.turn_b();
    MotorR.stop();
    do{
        w_time = 4 - timer.read();
    }while(w_time > 0);
    
    MotorL.stop();
    MotorR.stop();
    
    esc = false;
}

void escape2(){
    sendTick_XB.detach();
    sendTick_SD.detach();
    sendTick_RPi.detach();
    sendData();
    recData();
    sendRPi();
    sendTick_XB.attach(&sendData, 1.0f);
    sendTick_SD.attach(&recData, 1.0f);
    sendTick_RPi.attach(&sendRPi, 1.0f);
    
    esc = true;
    
    MotorL.break_stop();
    MotorR.break_stop();
    wait(0.5f);
    MotorL.turn_b();
    MotorR.turn_b();
    wait(0.5f);
    MotorL.turn_b();
    MotorR.stop();
    wait(90.0 * 5 / 1000);
    
    esc = false;
}

/************************************************
旋回
************************************************/
void activate(){
    sendTick_XB.detach();
    sendTick_SD.detach();
    sendTick_RPi.detach();
    sendData();
    recData();
    sendRPi();
    sendTick_XB.attach(&sendData, 1.0f);
    sendTick_SD.attach(&recData, 1.0f);
    sendTick_RPi.attach(&sendRPi, 1.0f);
    
    if(rtg > 0){
        MotorL.stop();
        MotorR.turn_a(1.0f);
        wait((float)rtg * 5 / 1000);
        w_time = 0;
    }
    else{
        MotorL.turn_a(0.8f);
        MotorR.stop();
        wait((float)rtg * -5 / 1000);
        w_time = 0;
    }
    
    MotorL.turn_a(0.8f);
    MotorR.turn_a(1.0f);
}

/************************************************
GPS座標読み取り
************************************************/
void readGPS(){
    double gps_lat_now = GPS.Latitude();
    double gps_lon_now = GPS.Longitude();
    if(gps_lat_now > 28.0f && gps_lat_now < 33.0f){
        if(gps_lon_now > 128.0f && gps_lon_now < 133.0f){
            gps_lat = gps_lat_now;
            gps_lon = gps_lon_now;
            raw_count ++;
            if(raw_count == 4){
                raw_count = 0;
            }
        }
    }
    
    gps_alt = GPS.Height();
    GPS.getUTC(utc);
    utc[3] += 9;
    if(utc[3] >= 24){
        utc[3] -= 24;
    }
    gps_knot = GPS.Knot();
    gps_deg = GPS.Degree();
    gps_sat = GPS.Number();
    
    ltg = setLTG();
    
    if(CanSat == 1 && flag_ave){ //待機モード時のみ
        GPSAverage();
    }
    
    if(CanSat == 4 && !esc){ //走行モード時のみ
        rtg = setRTG();
        if(raw_count == 0){//raw_count == 4でリセットだから1秒ごと
            reset_count ++;
            if(ltg < 15){
                w_time = 3 - reset_count;
            }
            else{
                w_time = 5 - reset_count;
            }
        }
    }
}

/************************************************
センサー読み取り
************************************************/
void readSensor(){
    float pres_now = LPS25H.getPres();
    if(pres_now > 500 && pres_now < 1500){
        pres = pres_now;
        temp = LPS25H.getTemp();
        alt = LPS25H.getAlt(pres_0, temp_0);
    }
    
    if(CanSat == 2){
        if(alt < 5.0f && alt > -10.0f){
            sep_count ++;
        }
    }
    
    ADXL375.getOutput(acc);
    gravity = sqrt(pow(acc[0], 2) + pow(acc[1], 2) + pow(acc[2], 2));
    
    vin = Vinpin.read() * 8.4;
}

/************************************************
0m地点での気圧・温度のリセット
************************************************/
void resetPT(){
    pres_0 = pres;
    temp_0 = temp;
    
    safety = true;
    
    save = !save;
    if(save){
        startRec();
    }
    else{
        stopRec();
    }
}

/************************************************
RPiからのデータ読み取り
************************************************/
void readRPi(){
    RPi_read[RPi_pt] = RPi.getc();
    RPi_pt ++;
    
    if(RPi_read[RPi_pt - 1] == '_'){
        if(RPi_pt > 3){
            RPi_read[RPi_pt - 1] = '\0';
            readCommand();
        }
        RPi_pt = 0;
    }
}

/************************************************
RPiにデータ送信
************************************************/
void sendRPi(){
    RPi.printf("%d", CanSat);
}

/************************************************
RPiコマンド解析
************************************************/
void readCommand(){
    char command[128];
    strcpy(command, RPi_read);
    sscanf(command, "%d,%d,%d,%d,%d,%d,%d", &red_per,
                                            &red_center_x, &red_center_y,
                                            &red_moment_x, &red_moment_y,
                                            &yel_per, &laser);
}

/************************************************
割り込み：アクチュエータチェック
************************************************/
void ConnectCheck(){
    sendTick_XB.detach();
    sendTick_SD.detach();
    sendTick_RPi.detach();
    
    FlightPin.rise(&stopInterruptIn);
    Button.fall(&stopInterruptIn);
    Button.rise(&stopInterruptIn);
    GPSAve.fall(&stopInterruptIn);
    GPSAve.rise(&stopInterruptIn);
    
    Timer timer;
    
    pc.printf("\r\nTEST MODE\r\n");
    XB.printf("\r\nTEST MODE\r\n");
    wait(1.0f);
    
    MotorL.turn_a();
    pc.printf("Motor Left : FRONT\r\n");
    XB.printf("Motor Left : FRONT\r\n");
    wait(1.0f);
    MotorL.turn_b();
    pc.printf("Motor Left : BACK\r\n");
    XB.printf("Motor Left : BACK\r\n");
    wait(1.0f);
    MotorL.stop();
    
    MotorR.turn_a();
    pc.printf("Motor Right : FRONT\r\n");
    XB.printf("Motor Right : FRONT\r\n");
    wait(1.0f);
    MotorR.turn_b();
    pc.printf("Motor Right : BACK\r\n");
    XB.printf("Motor Right : BACK\r\n");
    wait(1.0f);
    MotorR.stop();
    
    Nichrome1.fire(3.0f);
    timer.start();
    do{
        w_time = 6 - timer.read();
        pc.printf("Nichrome 1 : %d %d\r", w_time, Nichrome1.status);
        XB.printf("Nichrome 1 : %d %d\r", w_time, Nichrome1.status);
        if(w_time == 3){
            Nichrome1.fire_off();
        }
        wait(0.1f);
    }while(w_time > 0);
    timer.stop();
    timer.reset();
    pc.printf("\n");
    XB.printf("\n");
    
    Nichrome2.fire(3.0f);
    timer.start();
    do{
        w_time = 6 - timer.read();
        pc.printf("Nichrome 2 : %d %d\r", w_time, Nichrome2.status);
        XB.printf("Nichrome 2 : %d %d\r", w_time, Nichrome2.status);
        if(w_time == 3){
            Nichrome2.fire_off();
        }
        wait(0.1f);
    }while(w_time > 0);
    timer.stop();
    timer.reset();
    pc.printf("\n");
    XB.printf("\n");
    
    w_time = 0;
    
    pc.printf("Test Finish!\r\n\n");
    XB.printf("Test Finish!\r\n\n");
    
    sendTick_XB.attach(&sendData, 1.0f);
    sendTick_SD.attach(&recData, 1.0f);
    sendTick_RPi.attach(&sendRPi, 1.0f);
    
    FlightPin.rise(&flight);
    Button.fall(&buttonPush);
    Button.rise(&buttonRelease);
    GPSAve.fall(&startGPSAve);
    GPSAve.rise(&stopGPSAve);
}

/************************************************
割り込み：GPS平均計算開始
************************************************/
void startGPSAve(){
    if(!interrupt){
        sendTick_XB.detach();
        sendTick_SD.detach();
        
        flag_ave = true;
        lat_sum = 0;
        lon_sum = 0;
        csum = 0;
        
        interrupt = true;
    }
}

/************************************************
割り込み：GPS平均計算終了
************************************************/
void stopGPSAve(){
    if(interrupt){
        flag_ave = false;
        
        pc.printf("GPS Average\r\nCount : %.0lf\r\nLAT : %lf(N)\r\nLON : %lf(E)\r\n", csum, lat_ave, lon_ave);
        XB.printf("GPS Average\r\nCount : %.0lf\r\nLAT : %lf(N)\r\nLON : %lf(E)\r\n", csum, lat_ave, lon_ave);
        
        wait(5.0f);
        
        sendTick_XB.attach(&sendData, 1.0f);
        sendTick_SD.attach(&recData, 1.0f);
        
        interrupt = false;
    }
}

/************************************************
GPS平均計算
************************************************/
void GPSAverage(){
    lat_sum += gps_lat;
    lon_sum += gps_lon;
    csum ++;
    lat_ave = lat_sum / csum;
    lon_ave = lon_sum / csum;
    
    pc.printf("GPS AVE(%.0lf) : %lf(N) , %lf(E)\r\n", csum, lat_ave, lon_ave);
    XB.printf("GPS AVE(%.0lf) : %lf(N) , %lf(E)\r\n", csum, lat_ave, lon_ave);
}

/************************************************
ピン割り込み停止
************************************************/
void stopInterruptIn(){
    pc.printf("Stopped Interrupt\r\n");
}

/************************************************
割り込み：ボタンを押したとき
************************************************/
void buttonPush(){
    if(!interrupt && !button_push){
        timer.start();
        button_push = true;
        interrupt = true;
    }
}

/************************************************
割り込み：ボタンを離したとき
************************************************/
void buttonRelease(){
    if(interrupt && button_push){
        int time_button;
        button_push = false;
        time_button = timer.read();
        timer.stop();
        timer.reset();
        if(time_button >= 3){
            ConnectCheck();
        }
        else{
            resetPT();
        }
        interrupt = false;
    }
}

/************************************************
PCでデータ表示
************************************************/
void printData(){
    for(int c1 = 0; c1 < 20; c1 ++){
        pc.printf("*");
    }
    pc.printf("\r\n");
    pc.printf("MODE     -> ");
    switch(CanSat){
        case 0:
        pc.printf("Setting\r\n");
        break;
        case 1:
        pc.printf("Wait\r\n");
        break;
        case 2:
        pc.printf("Descend\r\n");
        break;
        case 3:
        pc.printf("Separate\r\n");
        break;
        case 4:
        pc.printf("GPS\r\n");
        break;
        case 5:
        pc.printf("Camera\r\n");
        break;
        case 6:
        pc.printf("Finish\r\n");
    }
    pc.printf("GPS      -> %3.7f(N), %3.7f(E), %d\r\n", gps_lat, gps_lon, gps_sat);
    pc.printf("TIME     -> %d:%d:%02.2f\r\n", (int)utc[3], (int)utc[4], utc[5]);
    pc.printf("LPS25H   -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt);
    pc.printf("W_TIME   -> %d[s]\r\n", w_time);
    pc.printf("Nichrome -> %d, %d\r\n", Nichrome1.status, Nichrome2.status);
    wait(0.05f);
    pc.printf("RTG      -> %d[degree]\r\n", rtg);
    pc.printf("LTG      -> %.2lf[m]\r\n", ltg);
    pc.printf("Motor    -> %.2f(L), %.2f(R)\r\n", MotorL.status, MotorR.status);
    pc.printf("Stack    -> %d\r\n", esc);
    pc.printf("Save     -> %d\r\n", save);
    pc.printf("Battery  -> %.2f\r\n", vin);
    pc.printf("Red PER  -> %d\r\n", red_per);
    pc.printf("Red TRI  -> %d, %d\r\n", red_center_x, red_center_y);
    pc.printf("Red MOM  -> %d, %d\r\n", red_moment_x, red_moment_y);
    pc.printf("Yellow   -> %d\r\n", yel_per);
    pc.printf("Laser    -> %d\r\n\n\n", laser);
}

/************************************************
地上局へデータ送信
************************************************/
void sendData(){
    if(CanSat == 2 || CanSat == 3){
        XB.printf("%d[MODE],\t", CanSat);
        XB.printf("%d[s],\t%.4f[hPa],\t%.2f[m],\t", w_time, pres, alt);
        XB.printf("%d,\t%d %d\r\n", sep_count, Nichrome1.status, Nichrome2.status);
    }
    else{
        for(int c1 = 0; c1 < 20; c1 ++){
            XB.printf("*");
        }
        XB.printf("\r\n");
        XB.printf("MODE     -> ");
        switch(CanSat){
            case 0:
            XB.printf("Setting\r\n");
            break;
            case 1:
            XB.printf("Wait\r\n");
            break;
            case 2:
            XB.printf("Descend\r\n");
            break;
            case 3:
            XB.printf("Separate\r\n");
            break;
            case 4:
            XB.printf("GPS\r\n");
            break;
            case 5:
            XB.printf("Camera\r\n");
            break;
            case 6:
            XB.printf("Finish\r\n");
        }
        XB.printf("GPS      -> %3.7f(N), %3.7f(E), %d\r\n", gps_lat, gps_lon, gps_sat);
        XB.printf("TIME     -> %d:%d:%02.2f\r\n", (int)utc[3], (int)utc[4], utc[5]);
        XB.printf("LPS25H   -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt);
        XB.printf("W_TIME   -> %d[s]\r\n", w_time);
        XB.printf("Nichrome -> %d, %d\r\n", Nichrome1.status, Nichrome2.status);
        wait(0.05f);
        XB.printf("RTG      -> %d[degree]\r\n", rtg);
        XB.printf("LTG      -> %.2lf[m]\r\n", ltg);
        XB.printf("Motor    -> %.2f(L), %.2f(R)\r\n", MotorL.status, MotorR.status);
        XB.printf("Stack    -> %d\r\n", esc);
        XB.printf("Save     -> %d\r\n", save);
        XB.printf("Battery  -> %.2f\r\n", vin);
        wait(0.05f);
        XB.printf("Red PER  -> %d\r\n", red_per);
        XB.printf("Red TRI  -> %d, %d\r\n", red_center_x, red_center_y);
        XB.printf("Red MOM  -> %d, %d\r\n", red_moment_x, red_moment_y);
        XB.printf("Yellow   -> %d\r\n", yel_per);
        XB.printf("Laser    -> %d\r\n\n\n", laser);
    }
}

/************************************************
ログ保存
************************************************/
void recData(){
    if(save){
        //時間情報
        fprintf(fp, "%d/%d/%d,", (int)utc[0], (int)utc[1], (int)utc[2]);  //日付 
        fprintf(fp, "%d:%02d:%02.2f,", (int)utc[3], (int)utc[4], utc[5]); //時間
        switch(CanSat){                                                   //モード
            case 0:
            fprintf(fp, "Setting,");
            break;
            case 1:
            fprintf(fp, "Wait,");
            break;
            case 2:
            fprintf(fp, "Descend,");
            break;
            case 3:
            fprintf(fp, "Separate,");
            break;
            case 4:
            fprintf(fp, "GPS,");
            break;
            case 5:
            fprintf(fp, "Camera,");
            break;
            case 6:
            fprintf(fp, "Finish,");
        }
        //センサ情報
        fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt);         //GPS座標
        fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg);                        //GPS速度
        fprintf(fp, "%d,", gps_sat);                                         //GPS衛星数
        fprintf(fp, "%.4f,%.2f,%.2f,", pres, temp, alt);                     //気圧センサ
        fprintf(fp, "%.2f,%.2f,%.2f,%.4f,", acc[0], acc[1], acc[2], gravity);//加速度センサ
        fprintf(fp, "%.2f,", vin);                                           //電源電圧
        //制御情報
        fprintf(fp, "%d,", w_time);      //待機時間
        fprintf(fp, "%d,", rtg);         //RadToGoal（ゴールの方向）
        fprintf(fp, "%.2lf,", ltg);      //LengthToGal（ゴールまでの距離）
        //アクチュエータ情報
        fprintf(fp, "%d,%d,", Nichrome1.status, Nichrome2.status);   //ニクロム線
        fprintf(fp, "%.2f,%.2f,", MotorL.status, MotorR.status);     //モーター
        fprintf(fp, "%d,", esc);                                      //スタックしているか
        //ラズパイ情報
        fprintf(fp, "%d,", red_per);                        //赤色の割合
        fprintf(fp, "%d,%d,", red_center_x, red_center_y);  //赤色三角形重心の座標
        fprintf(fp, "%d,%d,", red_moment_x, red_moment_y);  //赤色重心の座標
        fprintf(fp, "%d,", yel_per);
        fprintf(fp, "%d", laser);             //距離センサ
        fprintf(fp, "\r\n");
    }
}

/************************************************
記録開始
************************************************/
void startRec(){
    fp = fopen(file_name, "a");
    pc.printf("START SAVING\r\n");
    XB.printf("START SAVING\r\n");
    wait(1.0f);
}

/************************************************
記録終了
************************************************/
void stopRec(){
    fprintf(fp, "\r\n\n");
    fclose(fp);
    pc.printf("STOP SAVING\r\n");
    XB.printf("STOP SAVING\r\n");
    wait(1.0f);
}

/************************************************
セットアップ（最初に1回実行）
************************************************/
void setup(){
    wait(0.5f);
    char setup_string[512];
    char sprint_buff[64];
    
    pc.printf("\r\n\nSetting Start\r\n");
    XB.printf("\r\n\nSetting Start\r\n");
    strcat(setup_string, "\r\n\nSetting Start\r\n");
    
    //LPS25Hの設定
    LPS25H.begin(25);
    LPS25H.setFIFO(16);
    if(LPS25H.whoAmI() == 1){
        pc.printf("LPS25H : OK\r\n");
        XB.printf("LPS25H : OK\r\n");
        strcat(setup_string, "LPS25H : OK\r\n");
        pres_0 = LPS25H.getPres();
        temp_0 = LPS25H.getTemp();
    }
    else{
        pc.printf("LPS25H : NG...\r\n");
        XB.printf("LPS25H : NG...\r\n");
        strcat(setup_string, "LPS25H : NG...\r\n");
    }
    
    //ADXL375の設定
    ADXL375.setDataRate(ADXL375_3200HZ);
    if(ADXL375.whoAmI() == 1){
        pc.printf("ADXL375 : OK\r\n");
        XB.printf("ADXL375 : OK\r\n");
        strcat(setup_string, "ADXL375 : OK\r\n");
    }
    else{
        pc.printf("ADXL375 : NG...\r\n");
        XB.printf("ADXL375 : NG...\r\n");
        strcat(setup_string, "ADXL375 : NG...\r\n");
    }
    
    ADXL375.offset(-0.3f, -0.6f, 0.3f);
    
    //GPS受信待機
    if(waitGPS){
        w_time = 0;
        while(!GPS.gps_readable){
            pc.printf("GPS Waiting... : %d\r", w_time);
            XB.printf("GPS Waiting... : %d\r", w_time);
            w_time ++;
            wait(1.0f);
        }
        pc.printf("GPS Waiting... : %d -> OK!!\r\n", w_time);
        XB.printf("GPS Waiting... : %d -> OK!!\r\n", w_time);
        sprintf(sprint_buff, "GPS Waiting... : %d -> OK!!\r\n", w_time);
        strcat(setup_string, sprint_buff);
    }
    
    //RPi起動待機
    if(flag_RPi_setting){
        bool flag_RPi_ok = false;
        w_time = 0;
        while(!flag_RPi_ok){
            if(RPi.readable()){
                char c = RPi.getc();
                if(c == '_'){
                    flag_RPi_ok = true;
                }
            }
            pc.printf("RPi setting : %d\r", w_time);
            XB.printf("RPi setting : %d\r", w_time);
            w_time ++;
            wait(1.0f);
        }
        pc.printf("RPi setting : %d -> OK!!\r\n", w_time);
        XB.printf("RPi setting : %d -> OK!!\r\n", w_time);
        sprintf(sprint_buff, "RPi setting : %d -> OK!!\r\n", w_time);
        strcat(setup_string, sprint_buff);
    }
    
    //まとめ
    pc.printf("All setting finished!! -> Start!!\r\n");
    XB.printf("All setting finished!! -> Start!!\r\n");
    strcat(setup_string, "All setting finished!! -> Start!!\r\n");
    
    fp = fopen(file_name, "a");
    fprintf(fp, setup_string);
    fclose(fp);
    setup_string[0] = NULL;
}