File content as of revision 0:0239d9f7bf79:

#include "mbed.h"
#include "define.h"
#include "string"

Ticker timer;

RawSerial upper( p9, p10, 115200);
RawSerial under( p13, p14, 115200);
RawSerial con  ( p28, p27, 115200);
RawSerial pc( USBTX, USBRX, 115200);

DigitalIn sw1(p24);
DigitalIn sw2(p25);
DigitalIn sw3(p26);

DigitalOut led(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
//_________________________↓変数宣言

string BLE_str = "";
bool set_end = 0;
bool Connected = 0;
bool mldp_online = 0;
bool pre_sw1 = 0, pre_sw2 = 0, pre_sw3 = 0;
char x = 127, y = 127, rot = 127;
char global_mode = 0b00000000;
char up_recv = 0b00000000, und_recv = 0b00000000;
//___________________________↓各送信処理系

void BLE_send(string send_str){
    con.printf(send_str.c_str());
}

void upper_send(char mode){
    upper.putc(mode);
    upper.printf("\n");
}

void under_send(string send_str){
    under.putc(send_str[0]);
    under.putc(send_str[1]);
    under.putc(send_str[2]);
    under.putc(send_str[3]);
    under.printf("\n");
}
//____________________________↓BLEモジュール設定

void BLE_setup(){
    led2 = 1;
    wait(1.0);
    BLE_send("+\r");
    wait(0.5);
    BLE_send("SF,1\r");
    wait(0.5);
    BLE_send("SR,92000000\r");
    wait(0.5);
    BLE_send("R,1\r");
    wait(4.0);
    BLE_send("F\r");
    wait(0.5);
    BLE_send("X\r");
    wait(0.5);
    BLE_send("E,0,0491621AD835\r");
    set_end = 1;
}
//________________________________↓各受信割込み

void upper_recv(){
    static string up_str = "";
    char up_c = upper.getc();
    
    if(up_c == '\n'){
        up_recv = up_str[0];
        up_str = "";
    }
    else up_str += up_c;
}

void under_recv(){
    led4 = 1;
    static string und_str = "";
    static int und_co = 0;
    char und_c = under.getc();
    
    if(und_c == '\n'){
        und_recv = und_str[1];
        und_str = "";
    }
    else {
        if(und_co > 2) und_str = "";
        und_str += und_c;
        und_co++;
        }
}

void con_recv(){
    char bt_c = con.getc();
    static int con_co = 0;
    
    if (bt_c == 'd' && !Connected) {
        BLE_str += bt_c;
        Connected = 1;
        BLE_str = "";
    }
    if (Connected && mldp_online && bt_c == '\r'){
        if (BLE_str == "Connection End"){
            x = 127;
            y = 127;
            rot = 127;
            global_mode = EMERGENCY;
            Connected = 0;
            led = 1;
        }
        pc.printf(BLE_str.c_str());
        BLE_str == "";
    }
    if (Connected && mldp_online && bt_c == '\n' && con_co == 4){
        if(BLE_str[3] == EMERGENCY){
            x = 127;
            y = 127;
            rot = 127;
            global_mode = EMERGENCY;
            
        }
        else{
            x = BLE_str[0];
            y = BLE_str[1];
            rot = BLE_str[2];
            global_mode = BLE_str[3];
        }
        BLE_str = "";
    }
    else {
        if(con_co > 5){
            BLE_str = "";
            con_co = 0;
        }
        BLE_str += bt_c;
        con_co++;
    }
}
//_________________________________↓主要処理(周期割込み)

void timer_warikomi(){
    string und_send = "";
    char up_send = 0b00000000, und_mode = 0b00000000;
    static int time_co = 0, power = 0;
    
    if (Connected && !mldp_online && time_co > 10) {
        BLE_send("I\r");
        pc.printf(BLE_str.c_str());
        pc.printf("MLDP_ONLINE\n");
        mldp_online = 1;
        Connected = 1;
        BLE_str = "";
    }
    else if (Connected && mldp_online){
            
            if(global_mode == EMERGENCY){
                und_mode = global_mode;
                up_send = global_mode;
                x = 127;
                y = 127;
                rot = 127;
                led3 = 1;
                led4 = 1;
            }
            
            else if(global_mode & AUTOMATION){
                led2 = 1;
                x = 127;
                y = 127;
                rot = 127;
                und_mode |= AUTOMATION;
                up_send |= AUTOMATION;
                if(und_recv & POINTS_1){
                    if(up_recv & POSITIONS){
                        und_mode &= ~POINTS_1;
                        und_mode |= POINTS_2;
                    }
                    else if(und_recv & POSITIONS) up_send |= POSITIONS;
                    else up_send &= ~POSITIONS;
                }if(und_recv & POINTS_2){
                    if(up_recv & POSITIONS){
                        und_mode &= ~POINTS_2;
                        und_mode |= POINTS_3;
                    }
                    else if(und_recv & POSITIONS) up_send |= POSITIONS;
                    else up_send &= ~POSITIONS;
                }if(und_recv & POINTS_3){
                    if(up_recv & POSITIONS){
                        und_mode &= ~POINTS_3;
                    }
                    else if(und_recv & POSITIONS) up_send |= POSITIONS;
                    else up_send &= ~POSITIONS;
                }else if(und_recv & POSITIONS) {
                    und_mode |= POINTS_1;
                    up_send |= POINTS_1;
                }
                else{
                }
            }
            
            else{
                led = 1;
                led2 = 0;
                und_mode = 0b00000000;
                
                BLE_send("\n");
                if(global_mode & LOADING) up_send |= LOADING;
                if(global_mode & CB_COLLECT) up_send |= CB_COLLECT;
                else up_send &= ~CB_COLLECT;
            }
            
            und_send += x;
            und_send += y;
            und_send += rot;
            und_send += und_mode;
            
            under_send(und_send);
            upper_send(und_recv);
            pc.printf("x:%d\ty:%d\trot:%d\tmode:%d\n",und_send[0],und_send[1],und_send[2],und_recv);
            und_send = "";
    }
    else {
        time_co++;
    }
}
//______________________________________

int main() {
    pre_sw1 = sw1.read();
    pre_sw2 = sw2.read();
    pre_sw3 = sw3.read();
    
    BLE_setup();
    while(!set_end);
    
    timer.attach(&timer_warikomi, INT_TIME);
    upper.attach(&upper_recv, RawSerial::RxIrq);
    under.attach(&upper_recv, RawSerial::RxIrq);
    con.attach(&con_recv, RawSerial::RxIrq);
    while(true) {
    }
}