File content as of revision 0:cbbc3528eb59:

#include "mbed.h"

Serial bc(PA_9,PA_10);              // USART1 Bluetooth (Tx, Rx)
Serial pc(PA_2,PA_3);               // USART2 pc (Tx, Rx)

DigitalOut led(LED2);               // LED 
DigitalOut enable(PB_12);           // Motor enable
DigitalOut p1(PB_13);               // Motor dir1 up
DigitalOut p2(PB_14);               // Motor dir2 down

DigitalIn up(PC_8);                 // Burton Motor control
DigitalIn down(PC_9);

AnalogIn disSensor(PA_7);           // Distance Sensor

Ticker timer1;                      // 
Ticker timer2;                      // 

/*------------------ ReadData Variables -------------------*/

uint8_t             rx_buf[8];              // < _ _ _ _ _ _ > 8Byte
char                RX_command[4] = {0,};   //   _ _ _ 
char                RX_data[4] =  {0,};     //         _ _ _ 
char                flag_RX = 0; 
event_callback_t    Test;

int                 targetDis;

/*------------------ Callback Functions -------------------*/

void ReadData(int events)
{
    bc.printf("Read The Data!!\n");
    
    
    if (rx_buf[0] == '<' && rx_buf[7] == '>')
    {
        flag_RX = 1;
        
        RX_command[0] = rx_buf[1];
        RX_command[1] = rx_buf[2];
        RX_command[2] = rx_buf[3];
        RX_data[0] = rx_buf[4];
        RX_data[1] = rx_buf[5];
        RX_data[2] = rx_buf[6];
    }
        
      
}

void ControlLED(int mode)
{
    bc.printf("mode = %d\n",mode);
    
    switch (mode)
    {
        case 0 :
        
        led = 0;
        
        break;
        
        case 1 :
        led = 1;
        
        break;
        
        case 2 :
        bc.printf("ANYBARO");
        
        break;      
        
    }
    
}


void MotorTest(int mode)
{
    
    bc.printf("mode = %d\n",mode);
    
    switch (mode)
    
    {
        case 0 : //stop
    
            enable = 0;

            break;
    
        case 1 : // up
    
            enable = 1;
            p1 = 0;
            p2 = 1;
    
            break;
    
        case 2 : // down
        
            enable = 1;
            p1 = 1;
            p2 = 0;
        
            break;       
            
    }
              
}

void ControlAng()
{
    double d1 = disSensor.read();
    double d2 = 161.89*d1*d1*d1*d1-437.31*d1*d1*d1+440.89*d1*d1-207.4*d1+42.245;
    
    bc.printf("%1.3f\n", d1);
    
//    int ref_d = targetDis-d2;
//    
//    if(ref_d > 0.01)
//    {
//            enable = 1;
//            p1 = 1;
//            p2 = 0;
//            
//    }
//    
//    else if(ref_d < -0.01)
//    {
//                
//            enable = 1;
//            p1 = 0;
//            p2 = 1;
//                 
//    }
//    
//    else
//    {
//        
//            enable = 0;
//            timer1.detach();
//              
//    }
}

void MotorBurton()
{
       
        int a = up.read();
        int b = down.read();
        int c = a*2 + b;
        
        bc.printf("%d %d %d\n ",a,b,c); 
        
        switch(c)
        {
            case 0 : //stop
        
                enable = 0;
        
                break;
        
            case 1 : // down
        
                enable = 1;
                p1 = 0;
                p2 = 1;
        
                break;
        
            case 2 : // up
            
                enable = 1;
                p1 = 1;
                p2 = 0;
            
                break;
            
            case 3 : // stop
            
                enable = 1;
                p1 = 0;
                p2 = 0;
            
                break;
                
        }
       
            
    
}


int main()
{
    bc.baud(9600);
    bc.printf("START 180622 Ver. \n");
    
    up.mode(PullNone);
    down.mode(PullNone);
    
    int modeLED;
    int modeMotor;
    
    modeLED = 0;
    modeMotor = 0;
    
    timer2.attach(&MotorBurton,0.1);
    
    while(1)
    {
         
        bc.read(rx_buf, 8, ReadData , SERIAL_EVENT_RX_COMPLETE, '\n');
        
        // Readdata callback // int event = SERIAL_EVENT_RX_COMPLETE //       
            
        // analyze the recieved packets
        
        if (1 == flag_RX)
        {
            flag_RX = 0;
            
            if (0 == strcmp(RX_command,"LED"))
            {
                bc.printf("LED CONTROL MODE!!");
                modeLED = atoi(RX_data);
                ControlLED(modeLED);
            }
            
            else if (0 == strcmp(RX_command,"MOT"))
            {               
                
                bc.printf("MOTOR CONTROL MODE!!");
                modeMotor = atoi(RX_data);
                MotorTest(modeMotor);
                timer1.attach(&ControlAng,0.1);
                                
            }
            

            
//            else if (0 == strcmp(RX_command,"DIS"))
//            {               
//            
//                bc.printf("MOTOR DISTANCE CONTROL MODE!!");
//                targetDis = atoi(RX_data);
//                timer1.attach(&ControlAng,0.1);
//                            
//            }
                   
                    
        }    
        
        wait_ms(100);     
        
    }
    
}