File content as of revision 8:075a9610e2cd:

#include "mbed.h"
#include "rtos.h"
DigitalOut l1(LED1);
DigitalOut l2(LED2);
DigitalOut l3(LED3);
DigitalOut l4(LED4);
Thread servo;
//Mutex stdio_mutex;
RawSerial motor(PC_4,PC_5);
unsigned char tx_buffer[3];
unsigned char tx_buffer_ptr = 0;
unsigned char tx_buffer_full = false;
unsigned char rx_buffer[6];
unsigned char rx_buffer_ptr = 0;
unsigned char rx_buffer_full = false;
signed short int motorcp[] = {8500,8500};
unsigned short int Position;
int MotorID;
void Tx_interrupt();
void Rx_interrupt();
void motor_update(unsigned char Id, unsigned short int Position)
{
    unsigned char id,lo,hi;
    id=0x80|Id;
    hi=(Position>>7)&0x007F;
    lo=Position&0x007F;
    NVIC_DisableIRQ(USART3_IRQn);
    tx_buffer[0] = id;
    tx_buffer[1] = hi;
    tx_buffer[2] = lo;
//    wait(0.0008);
    NVIC_EnableIRQ(USART3_IRQn);
}
void tx_thread() 
{
//    stdio_mutex.lock();
    for(MotorID=0;MotorID<=1;MotorID++)
    {
        motor_update(MotorID,motorcp[MotorID]);
        wait_us(900);
    }
//    stdio_mutex.unlock();
}
void SERVO_thread() 
{
    motor.format(8,Serial::Even,1);
    motor.baud(115200);
    rx_buffer_ptr = 0;
    rx_buffer_full = false;
    motor.attach(&Rx_interrupt, Serial::RxIrq);
    motor.attach(&Tx_interrupt, Serial::TxIrq);
    NVIC_EnableIRQ(USART3_IRQn);
    RtosTimer motorposition(tx_thread);
    motorposition.start(10);
    while(1)
    {
        motorcp[0]=7500;
        motorcp[1]=7500;
        Thread::wait(1000);
        motorcp[0]=8500;
        motorcp[1]=8500;
        Thread::wait(1000);
    }
}
int main()
{   
    servo.start(SERVO_thread);     
    while (1) 
    { 
        l1=!l1;
        Thread::wait(1000);
//       printf("Tx : %d %x %x\r\n",tx_buffer[0],tx_buffer[1],tx_buffer[2]);
//        printf("RX : %d %x %x %d %x %x \r\n",rx_buffer[0], rx_buffer[1],rx_buffer[2],rx_buffer[3],rx_buffer[4],rx_buffer[5]);  
    }
}
void Tx_interrupt() 
{
    while(motor.writeable())
    {    
        motor.putc(tx_buffer[tx_buffer_ptr]);
        tx_buffer_ptr++;
        if(tx_buffer_ptr==3)
              tx_buffer_ptr = 0;
    }
    return;
}
void Rx_interrupt()
{
    while(motor.readable())
    {
        rx_buffer[rx_buffer_ptr] = motor.getc();
        rx_buffer_ptr++;
        if(rx_buffer_ptr==6)
        {
            rx_buffer_full = true;
            rx_buffer_ptr=0;
        }
    }
}