File content as of revision 5:1faeeab28bd1:

#include "mbed.h"
#include "kondo1.h"
#include "servo.h"
#define KONDO1_COMMAND_POSITION_FREE 1
#define KONDO1_COMMAND_POSITION_SET 2
#define KONDO1_STEP_PER_DEGREE 8000/270
RawSerial KONDO1_serial(PC_4,PC_5);
unsigned char KONDO1_Buffer_Tx[3];
unsigned char KONDO1_Buffer_Tx_Ptr = 0;
unsigned char KONDO1_Buffer_Tx_Full = false;
unsigned char KONDO1_Buffer_Rx[6];
unsigned char KONDO1_Buffer_Rx_Ptr = 3;
unsigned char KONDO1_Buffer_Rx_Full = false;
signed short int KONDO1_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500};
signed short int KONDO1_Positions_Target[KONDO1_MAX_TOTAL];
//signed short int KONDO1_Positions_Current[KONDO1_MAX_TOTAL]= {7500,7500,7500,7500,7500};
unsigned char KONDO1_SendOrder[]= {1,2,3,4,5,6};
signed char KONDO1_Directions = 1;
signed char KONDO1_ID = 0;
signed char KONDO1_PtrCommands_Set;
EventQueue KONDO1_queue;
/*signed short int KONDO1_Position_Convert(char hi, char lo)
{
    signed short int i = hi;
    i = i<<7;
    i = i|lo;
    return i;
}
void KONDO1_Position_Current_Set()
{
    unsigned char id;
    id = KONDO1_Buffer_Rx[0]&0x1F;
    KONDO1_Positions_Current[id] = KONDO1_Position_Convert(KONDO1_Buffer_Rx[4], KONDO1_Buffer_Rx[5]);
}*/
void KONDO1_Positions_Set(double* KONDO1_Degrees)
{
    unsigned char id;
    for(KONDO1_ID=0;KONDO1_ID < sizeof(KONDO1_SendOrder);KONDO1_ID++)
    {
        id = KONDO1_SendOrder[KONDO1_ID];
//            switch(KONDO1_PtrCommands_Set)
//            {
//                case KONDO1_COMMAND_POSITION_FREE:
//                    KONDO1_Positions[id]=0;
//                    break;
//                case KONDO1_COMMAND_POSITION_SET:
        KONDO1_Positions_Target[id] = KONDO1_Degrees[id] * KONDO1_STEP_PER_DEGREE * KONDO1_Directions + KONDO1_Positions_Initial[id];
//                    break;
//                  case KONDO1_COMMAND_DEGREE_SET:
//                    KONDO1_degrees_Target[id] = (KONDO1_Positions_Target[id] - KONDO1_Positions_Initial[id])/ (KONDO1_STEP_PER_DEGREE * KONDO1_Directions);
//                    break;
//wait(0.000625);
    }
}
void KONDO1_Interrupt_Tx()
{
    while(KONDO1_serial.writeable())
    {
        KONDO1_serial.putc(KONDO1_Buffer_Tx[KONDO1_Buffer_Tx_Ptr]);
        KONDO1_Buffer_Tx_Ptr++;
        if(KONDO1_Buffer_Tx_Ptr==3)
              KONDO1_Buffer_Tx_Ptr = 0;
    }
}
void KONDO1_Interrupt_Rx()
{
    while(KONDO1_serial.readable())
    {
        KONDO1_Buffer_Rx[KONDO1_Buffer_Rx_Ptr] = KONDO1_serial.getc();
        KONDO1_Buffer_Rx_Ptr++;
        if(KONDO1_Buffer_Rx_Ptr==6)
        {
            KONDO1_Buffer_Rx_Full = true;
            KONDO1_Buffer_Rx_Ptr=0;
//            KONDO1_Position_Current_Set();
        }
    }
} 
void KONDO1_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);
    KONDO1_Buffer_Tx[0] = id;// KONDO1_Buffer_Rx[0] = KONDO1_serial.getc();
    KONDO1_Buffer_Tx[1] = hi;// KONDO1_Buffer_Rx[1] = KONDO1_serial.getc();
    KONDO1_Buffer_Tx[2] = lo;// KONDO1_Buffer_Rx[2] = KONDO1_serial.getc();
//    wait(0.0007);      
    KONDO1_Buffer_Rx_Full = false;
    NVIC_EnableIRQ(USART3_IRQn);
}
void KONDO1_task() 
{
    signed char id;
    signed char i;
    for(i=0 ; i < sizeof(KONDO1_SendOrder) ; i++)
    {
        id = KONDO1_SendOrder[i];
//        if(id==0)KONDO1_update(id, 0);else
        KONDO1_update(id, KONDO1_Positions_Target[id]);
        wait_us(650);    //650   1000
//        printf("%d=%d\r\n",id,KONDO1_Positions_Current[id]);
//        printf("%x,%x,%x,%x,%x,%x\r\n",KONDO1_Buffer_Rx[0],KONDO1_Buffer_Rx[1],KONDO1_Buffer_Rx[2],KONDO1_Buffer_Rx[3],KONDO1_Buffer_Rx[4],KONDO1_Buffer_Rx[5]);
    }
}
void KONDO1_init()
{
    KONDO1_serial.format(8,Serial::Even,1);
    KONDO1_serial.baud(115200);
    KONDO1_Buffer_Rx_Ptr = 0;
    KONDO1_Buffer_Rx_Full = false;
    KONDO1_serial.attach(&KONDO1_Interrupt_Rx, Serial::RxIrq);
    KONDO1_serial.attach(&KONDO1_Interrupt_Tx, Serial::TxIrq);
    NVIC_EnableIRQ(USART3_IRQn);
    KONDO1_queue.call_every(10, KONDO1_task);   //1000    0.1  1
    KONDO1_queue.dispatch();
}