kondo3.cpp

00001 #include "mbed.h"
00002 #include "kondo3.h"
00003 #include "servo.h"
00004 #define KONDO3_COMMAND_POSITION_FREE 1
00005 #define KONDO3_COMMAND_POSITION_SET 2
00006 #define KONDO3_STEP_PER_DEGREE 8000/270
00007 RawSerial KONDO3_serial(PA_0,PA_1);
00008 unsigned char KONDO3_Buffer_Tx[3];
00009 unsigned char KONDO3_Buffer_Tx_Ptr = 0;
00010 unsigned char KONDO3_Buffer_Tx_Full = false;
00011 unsigned char KONDO3_Buffer_Rx[6];
00012 unsigned char KONDO3_Buffer_Rx_Ptr = 0;
00013 unsigned char KONDO3_Buffer_Rx_Full = false;
00014 signed short int KONDO3_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
00015 signed short int KONDO3_Positions_Current[KONDO3_MAX_TOTAL];
00016 signed short int KONDO3_Positions_Target[KONDO3_MAX_TOTAL];
00017 unsigned char KONDO3_SendOrder[]= {16,15,14,13,12,11};
00018 signed char KONDO3_Directions = 1;
00019 signed char KONDO3_ID = 0;
00020 signed char KONDO3_PtrCommands_Set;
00021 EventQueue KONDO3_queue;
00022 void KONDO3_Positions_Set(double* KONDO3_Degrees)
00023 {
00024     unsigned char id;
00025     for(KONDO3_ID=0;KONDO3_ID < sizeof(KONDO3_SendOrder);KONDO3_ID++)
00026     {
00027     id = KONDO3_SendOrder[KONDO3_ID];
00028 //            switch(KONDO1_PtrCommands_Set)
00029 //            {
00030 //                case KONDO1_COMMAND_POSITION_FREE:
00031 //                    KONDO1_Positions[id]=0;
00032 //                    break;
00033 //                KONDO1_COMMAND_POSITION_SET:
00034     KONDO3_Positions_Target[id] = KONDO3_Degrees[id] * KONDO3_STEP_PER_DEGREE * KONDO3_Directions + KONDO3_Positions_Initial[id];
00035 //    wait(0.0008);
00036     }  
00037 }
00038 void KONDO3_Interrupt_Tx() 
00039 {
00040     while(KONDO3_serial.writeable())
00041     {
00042         KONDO3_serial.putc(KONDO3_Buffer_Tx[KONDO3_Buffer_Tx_Ptr]);
00043         KONDO3_Buffer_Tx_Ptr++;
00044         if(KONDO3_Buffer_Tx_Ptr==3)
00045               KONDO3_Buffer_Tx_Ptr = 0;
00046     }
00047 }
00048 void KONDO3_Interrupt_Rx()
00049 {
00050     while(KONDO3_serial.readable())
00051     {
00052         KONDO3_Buffer_Rx[KONDO3_Buffer_Rx_Ptr] = KONDO3_serial.getc();
00053         KONDO3_Buffer_Rx_Ptr++;
00054         if(KONDO3_Buffer_Rx_Ptr==6)
00055         {
00056             KONDO3_Buffer_Rx_Full = true;
00057             KONDO3_Buffer_Rx_Ptr=0;
00058         }
00059     }
00060 } 
00061 void KONDO3_update(unsigned char Id, unsigned short int Position)
00062 {
00063     unsigned char id,lo,hi;
00064     id=0x80|Id;
00065     hi=(Position>>7)&0x007F;
00066     lo=Position&0x007F;
00067     NVIC_DisableIRQ(USART1_IRQn);
00068     KONDO3_Buffer_Tx[0] = id;
00069     KONDO3_Buffer_Tx[1] = hi;
00070     KONDO3_Buffer_Tx[2] = lo;
00071 //    wait(0.0008);
00072     KONDO3_Buffer_Rx_Full = false;
00073     NVIC_EnableIRQ(USART1_IRQn);
00074 }
00075 void KONDO3_task() 
00076 {
00077     signed char id;
00078     signed char i;
00079     for(i=0 ; i < sizeof(KONDO3_SendOrder) ; i++)
00080     {
00081         id = KONDO3_SendOrder[i];
00082         KONDO3_update(id, KONDO3_Positions_Target[id]);
00083         wait_us(650);
00084 //        wait_us(825);
00085     }
00086 }
00087 void KONDO3_init() 
00088 {
00089     KONDO3_serial.format(8,Serial::Even,1);
00090     KONDO3_serial.baud(115200);
00091     KONDO3_Buffer_Rx_Ptr = 0;
00092     KONDO3_Buffer_Rx_Full = false;
00093     KONDO3_serial.attach(&KONDO3_Interrupt_Rx, Serial::RxIrq);
00094     KONDO3_serial.attach(&KONDO3_Interrupt_Tx, Serial::TxIrq);
00095     NVIC_EnableIRQ(USART1_IRQn);
00096     KONDO3_queue.call_every(10, KONDO3_task); //1000
00097     KONDO3_queue.dispatch();
00098 }