File content as of revision 4:99891561a38b:

#include "mbed.h"
#include "kondo2.h"
#include "servo.h"
#define KONDO2_COMMAND_POSITION_FREE 1
#define KONDO2_COMMAND_POSITION_SET 2
#define KONDO2_STEP_PER_DEGREE 8000/270
RawSerial KONDO2_serial(PA_2,PA_3);
unsigned char KONDO2_Buffer_Tx[3];
unsigned char KONDO2_Buffer_Tx_Ptr = 0;
unsigned char KONDO2_Buffer_Tx_Full = false;
unsigned char KONDO2_Buffer_Rx[6];
unsigned char KONDO2_Buffer_Rx_Ptr = 0;
unsigned char KONDO2_Buffer_Rx_Full = false;
signed short int KONDO2_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
signed short int KONDO2_Positions_Current[KONDO2_MAX_TOTAL];
signed short int KONDO2_Positions_Target[KONDO2_MAX_TOTAL];
unsigned char KONDO2_SendOrder[]= {7,9,10};
signed char KONDO2_Directions = 1;
signed char KONDO2_PtrCommands_Set;
signed char KONDO2_ID = 0;
EventQueue KONDO2_queue;
void KONDO2_Positions_Set(double* KONDO2_Degrees)
{
        unsigned char id;
        for(KONDO2_ID=0;KONDO2_ID < sizeof(KONDO2_SendOrder);KONDO2_ID++)
        {
        id = KONDO2_SendOrder[KONDO2_ID];
        KONDO2_Positions_Target[id] = KONDO2_Degrees[id] * KONDO2_STEP_PER_DEGREE * KONDO2_Directions + KONDO2_Positions_Initial[id];
//        wait(0.0008);
        }
}
void KONDO2_Interrupt_Tx() 
{
    while(KONDO2_serial.writeable())
    {
        KONDO2_serial.putc(KONDO2_Buffer_Tx[KONDO2_Buffer_Tx_Ptr]);
//        KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
        KONDO2_Buffer_Tx_Ptr++;
 //       KONDO2_Buffer_Rx_Ptr++;
        if(KONDO2_Buffer_Tx_Ptr==3 && KONDO2_Buffer_Rx_Ptr==3)
              KONDO2_Buffer_Tx_Ptr = 0;
//              KONDO2_Buffer_Rx_Ptr = 0;
    }
}
void KONDO2_Interrupt_Rx()
{
    while(KONDO2_serial.readable())
    {
 //       KONDO2_Buffer_Rx_Ptr = 3;
        KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
        KONDO2_Buffer_Rx_Ptr++;
        if(KONDO2_Buffer_Rx_Ptr==6)
        {
            KONDO2_Buffer_Rx_Full = true;
            KONDO2_Buffer_Rx_Ptr=0;
        }
    }
} 
void KONDO2_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(USART2_IRQn);
    KONDO2_Buffer_Tx[0] = id;
    KONDO2_Buffer_Tx[1] = hi;
    KONDO2_Buffer_Tx[2] = lo;
//    wait(0.0008);
    KONDO2_Buffer_Rx_Full = false;
    NVIC_EnableIRQ(USART2_IRQn);
}
void KONDO2_task() 
{
    signed char id;
    signed char i;
    for(i=0 ; i < sizeof(KONDO2_SendOrder) ; i++)
    {
        id = KONDO2_SendOrder[i];
        KONDO2_update(id, KONDO2_Positions_Target[id]);
//        wait_us(825);
        wait_us(650);
    }
}
void KONDO2_init() 
{
    KONDO2_serial.format(8,Serial::Even,1);
    KONDO2_serial.baud(115200);
    KONDO2_Buffer_Rx_Ptr = 0;
    KONDO2_Buffer_Rx_Full = false;
    KONDO2_serial.attach(&KONDO2_Interrupt_Rx, Serial::RxIrq);
    KONDO2_serial.attach(&KONDO2_Interrupt_Tx, Serial::TxIrq);
    NVIC_EnableIRQ(USART2_IRQn);    
    KONDO2_queue.call_every(10, KONDO2_task);
    KONDO2_queue.dispatch();
}