File content as of revision 0:36e2fa5ac56d:

#include "mbed.h"
#include "HCSR04.h"

struct UART_buf
{ 
    uint8_t STA;
    uint8_t MODE; 
    uint8_t CMD;
    uint8_t LEN;
    uint8_t DATA[32];
    uint8_t END; 
     
};
 
// INA, INB
PinName pin_DC[2] = {PA_6, PA_7};
PinName pin_ECHO = PB_7;
PinName pin_TRIG = PB_10;

PwmOut INA(pin_DC[0]);
PwmOut INB(pin_DC[1]);
HCSR04 Ultrasonic(pin_ECHO, pin_TRIG);

Serial SerialUART(PA_2, PA_3);

enum{STOP, CW, CCW};

uint8_t Buffer[37];

UART_buf RX_BUF;

Timer Sensor_Timer;

uint32_t T_period = 1000;
uint32_t timer_check = 0;

void SerialUARTRX_ISR(void);
void DC_Ctrl(uint8_t dir, float Speed);
void Timer_setting(uint8_t cmd, uint8_t value);
void Sensor_Read(void);
float Ultrasonic_cm(void);

int main() {
    
    SerialUART.baud(115200);
    
    SerialUART.attach(&SerialUARTRX_ISR);
    
    Timer_setting(0x06, 1);
    
    while(1)
    {
        //SerialUART.printf("%d", Sensor_Timer.read_ms());
        if((timer_check + T_period) < Sensor_Timer.read_ms())
        {
          //  SerialUART.printf("%d",T_period);
            Sensor_Read();
            timer_check += T_period;
        }
        //wait(0.5);
    }
}

void SerialUARTRX_ISR(void)
{
    static uint8_t RX_count = 0, RX_Len = 32, RX_Status = 0;
    uint8_t rx_da = SerialUART.getc();
    switch(RX_Status)
    {
        case 0:
            if(rx_da == 0x76)
            {
                RX_BUF.STA = rx_da;
                RX_Status++;
            }
            break;
        case 1:
            RX_BUF.MODE = rx_da;
            RX_Status++;
            break;
        case 2:
            RX_BUF.CMD = rx_da;
            RX_Status++;
            break;
        case 3:
            RX_BUF.LEN = rx_da;
            RX_Len = RX_BUF.LEN;
            RX_Status++;
            if(RX_Len == 0)
                RX_Status++;
            break;
        case 4:
            RX_BUF.DATA[RX_count] = rx_da;
            RX_count++;
            if(RX_count == RX_Len)
            {
                RX_Status++;
                RX_count = 0;
                RX_Len = 32;
            }
            break;
        case 5:
            if(rx_da == 0x3E)
            {
                RX_BUF.END = rx_da;
                RX_Status = 0;
                switch(RX_BUF.MODE)
                {
                    case 0x01:
                        if(RX_BUF.CMD == 0x12)
                        {
                            DC_Ctrl(RX_BUF.DATA[0], 50);
                        }
                        break;
                    case 0x04:
                        Timer_setting(RX_BUF.CMD, RX_BUF.DATA[0]);
                        break;
                }
            }
            break;
    }
}

void DC_Ctrl(uint8_t dir, float Speed)
{
    switch(dir)
    {
        case STOP:
            INA = 0;
            INB = 0;
            break;
        case CW:
            INA = Speed/100;
            INB = 0;
            break;
        case CCW:
            INA = 0;
            INB = Speed/100;
            break;
    }
}

void Timer_setting(uint8_t cmd, uint8_t value)
{
    double Time_value = 0;
    Sensor_Timer.stop();
    Sensor_Timer.reset();
    switch(cmd)
    {
        case 0x01:
            Time_value = 30;
            break;
        case 0x02:
            Time_value = 60;
            break;
        case 0x03:
            Time_value = 120;
            break;
        case 0x04:
            Time_value = 300;
            break;
        case 0x05:
            Time_value = 600;
            break;
        case 0x06:
            Time_value = value;
            Time_value = 1.0/Time_value;
            break;
    }
    Sensor_Timer.start();
    timer_check = Sensor_Timer.read_ms();
    T_period = Time_value * 1000;
}

void Sensor_Read(void)
{
    uint16_t dist = Ultrasonic_cm();
    Buffer[0] = 0x76;
    Buffer[1] = 0x01;
    Buffer[2] = 0x07;
    Buffer[3] = 0x02;
    Buffer[4] = dist >> 8;
    Buffer[5] = dist & 0xFF;
    Buffer[6] = 0x3E;
    for(int i=0; i<7; i++)
        SerialUART.putc(Buffer[i]);
}

float Ultrasonic_cm(void)
{
    Ultrasonic.startMeasurement();
    
    while(!Ultrasonic.isNewDataReady());
    
    return Ultrasonic.getDistance_cm();
}