File content as of revision 4:45bd9ba6c0da:

#include "mbed.h"
#include "kondo3.h"
#include "servo.h"
#define KONDO3_COMMAND_POSITION_FREE 1
#define KONDO3_COMMAND_POSITION_SET 2
#define KONDO3_STEP_PER_DEGREE 8000/270
RawSerial KONDO3_serial(PA_0,PA_1);
unsigned char KONDO3_Buffer_Tx[3];
unsigned char KONDO3_Buffer_Tx_Ptr = 0;
unsigned char KONDO3_Buffer_Tx_Full = false;
unsigned char KONDO3_Buffer_Rx[6];
unsigned char KONDO3_Buffer_Rx_Ptr = 0;
unsigned char KONDO3_Buffer_Rx_Full = false;
signed short int KONDO3_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
signed short int KONDO3_Positions_Current[KONDO3_MAX_TOTAL];
signed short int KONDO3_Positions_Target[KONDO3_MAX_TOTAL];
unsigned char KONDO3_SendOrder[]= {16,15,14,13,12,11};
signed char KONDO3_Directions = 1;
signed char KONDO3_ID = 0;
signed char KONDO3_PtrCommands_Set;
EventQueue KONDO3_queue;
void KONDO3_Positions_Set(double* KONDO3_Degrees)
{
    unsigned char id;
    for(KONDO3_ID=0;KONDO3_ID < sizeof(KONDO3_SendOrder);KONDO3_ID++)
    {
    id = KONDO3_SendOrder[KONDO3_ID];
//            switch(KONDO1_PtrCommands_Set)
//            {
//                case KONDO1_COMMAND_POSITION_FREE:
//                    KONDO1_Positions[id]=0;
//                    break;
//                KONDO1_COMMAND_POSITION_SET:
    KONDO3_Positions_Target[id] = KONDO3_Degrees[id] * KONDO3_STEP_PER_DEGREE * KONDO3_Directions + KONDO3_Positions_Initial[id];
//    wait(0.0008);
    }  
}
void KONDO3_Interrupt_Tx() 
{
    while(KONDO3_serial.writeable())
    {
        KONDO3_serial.putc(KONDO3_Buffer_Tx[KONDO3_Buffer_Tx_Ptr]);
        KONDO3_Buffer_Tx_Ptr++;
        if(KONDO3_Buffer_Tx_Ptr==3)
              KONDO3_Buffer_Tx_Ptr = 0;
    }
}
void KONDO3_Interrupt_Rx()
{
    while(KONDO3_serial.readable())
    {
        KONDO3_Buffer_Rx[KONDO3_Buffer_Rx_Ptr] = KONDO3_serial.getc();
        KONDO3_Buffer_Rx_Ptr++;
        if(KONDO3_Buffer_Rx_Ptr==6)
        {
            KONDO3_Buffer_Rx_Full = true;
            KONDO3_Buffer_Rx_Ptr=0;
        }
    }
} 
void KONDO3_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(USART1_IRQn);
    KONDO3_Buffer_Tx[0] = id;
    KONDO3_Buffer_Tx[1] = hi;
    KONDO3_Buffer_Tx[2] = lo;
//    wait(0.0008);
    KONDO3_Buffer_Rx_Full = false;
    NVIC_EnableIRQ(USART1_IRQn);
}
void KONDO3_task() 
{
    signed char id;
    signed char i;
    for(i=0 ; i < sizeof(KONDO3_SendOrder) ; i++)
    {
        id = KONDO3_SendOrder[i];
        KONDO3_update(id, KONDO3_Positions_Target[id]);
        wait_us(650);
//        wait_us(825);
    }
}
void KONDO3_init() 
{
    KONDO3_serial.format(8,Serial::Even,1);
    KONDO3_serial.baud(115200);
    KONDO3_Buffer_Rx_Ptr = 0;
    KONDO3_Buffer_Rx_Full = false;
    KONDO3_serial.attach(&KONDO3_Interrupt_Rx, Serial::RxIrq);
    KONDO3_serial.attach(&KONDO3_Interrupt_Tx, Serial::TxIrq);
    NVIC_EnableIRQ(USART1_IRQn);
    KONDO3_queue.call_every(10, KONDO3_task); //1000
    KONDO3_queue.dispatch();
}