File content as of revision 3:b79aa7d836fb:

#include "mbed.h"

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

InterruptIn up(PC_8);         // Button Interrupt Motor control
InterruptIn down(PC_9);


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

AnalogIn disSensor(PA_7);           // Distance Sensor

AnalogIn force_L1(PB_0);            // Force Sensor
AnalogIn force_L2(PB_1);
AnalogIn force_L3(PC_0);            // Force Sensor
AnalogIn force_L4(PC_1);
AnalogIn force_R1(PC_2);            // Force Sensor
AnalogIn force_R2(PC_3);
AnalogIn force_R3(PC_4);            // Force Sensor
AnalogIn force_R4(PC_5);


Ticker timer0;                      // Operating MCU LED on 
Ticker timer1;                      // Control Angle By Using Distance Sensor
Ticker timer2;                      // Force Sensor

/*------------------ 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()
{
        
        float d1 = disSensor.read();
        float d2 = -840.53f*d1*d1*d1+1742.1f*d1*d1-1262.7f*d1+340.25f;
//        float d2 = -840.53f*d1*d1*d1+1742.1f*d1*d1-1600.0f*d1+340.25f;
        float ref_d = targetDis-d2;
        
        bc.printf("%1.3f %1.3f %1.3f\n", d1, d2, ref_d);
        
        if(ref_d > 0.9f)
        {
                enable = 1;
                p1 = 0;
                p2 = 1;
                
        }
        
        else if(ref_d < -0.9f)
        {
                    
                enable = 1;
                p1 = 1;
                p2 = 0;
                     
        }
        
        else
        {
            
                enable = 0;
                timer1.detach();
                  
        }
}

void MotorButton()
{
        
        int a = up.read();
        int b = down.read();
        int c = a*2 + b;
        
        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;
                
        }
       
            
    
}

void ReadForce()
{
    float force_L[4];
    float force_R[4];
    
    force_L[0] = force_L1.read();
    force_L[1] = force_L2.read();
    force_L[2] = force_L3.read();
    force_L[3] = force_L4.read();
    force_R[0] = force_R1.read();
    force_R[1] = force_R2.read();
    force_R[2] = force_R3.read();
    force_R[3] = force_R4.read();
    
//    bc.printf("%1.3f, %1.3f, %1.3f, %1.3f\n", force_L[0],force_L[1],force_L[2],force_L[3]);
//    bc.printf("%1.3f, %1.3f, %1.3f, %1.3f\n", force_R[0],force_R[1],force_R[2],force_R[3]);
}

void OperateLED()
{
    led =!led;
}

int main()
{
    bc.baud(9600);
    bc.printf("START 180622 Ver. \n");
    led = 1;  
    
    up.mode(PullNone);
    down.mode(PullNone);
    
    up.fall(&MotorButton);                  // 1 > 0 swtich on 
    up.rise(&MotorButton);                  // 0 > 1 switch on
    down.fall(&MotorButton);
    down.rise(&MotorButton);
    
    int modeLED;
    int modeMotor;
    
    modeLED = 0;
    modeMotor = 0;
    
    timer0.attach(&OperateLED,0.5);
    timer2.attach(&ReadForce,0.05);
    
    
    
    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 TEST CONTROL MODE!!");
                modeMotor = atoi(RX_data);
                MotorTest(modeMotor);              
                                
            }
            
            else if (0 == strcmp(RX_command,"POS"))
            {
                bc.printf("MOTOR DISTANCE CONTROL MODE!!");          
                targetDis = atoi(RX_data);
                timer1.attach(&ControlAng,0.1);
                            
            }
                   
                    
        }    
           
        
    }
    
}