kondo2.cpp

00001 #include "mbed.h"
00002 #include "kondo2.h"
00003 #include "servo.h"
00004 #define KONDO2_COMMAND_POSITION_FREE 1
00005 #define KONDO2_COMMAND_POSITION_SET 2
00006 #define KONDO2_STEP_PER_DEGREE 8000/270
00007 RawSerial KONDO2_serial(PA_2,PA_3);
00008 unsigned char KONDO2_Buffer_Tx[3];
00009 unsigned char KONDO2_Buffer_Tx_Ptr = 0;
00010 unsigned char KONDO2_Buffer_Tx_Full = false;
00011 unsigned char KONDO2_Buffer_Rx[6];
00012 unsigned char KONDO2_Buffer_Rx_Ptr = 0;
00013 unsigned char KONDO2_Buffer_Rx_Full = false;
00014 signed short int KONDO2_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
00015 signed short int KONDO2_Positions_Current[KONDO2_MAX_TOTAL];
00016 signed short int KONDO2_Positions_Target[KONDO2_MAX_TOTAL];
00017 unsigned char KONDO2_SendOrder[]= {7,9,10};
00018 signed char KONDO2_Directions = 1;
00019 signed char KONDO2_PtrCommands_Set;
00020 signed char KONDO2_ID = 0;
00021 EventQueue KONDO2_queue;
00022 void KONDO2_Positions_Set(double* KONDO2_Degrees)
00023 {
00024         unsigned char id;
00025         for(KONDO2_ID=0;KONDO2_ID < sizeof(KONDO2_SendOrder);KONDO2_ID++)
00026         {
00027         id = KONDO2_SendOrder[KONDO2_ID];
00028         KONDO2_Positions_Target[id] = KONDO2_Degrees[id] * KONDO2_STEP_PER_DEGREE * KONDO2_Directions + KONDO2_Positions_Initial[id];
00029 //        wait(0.0008);
00030         }
00031 }
00032 void KONDO2_Interrupt_Tx() 
00033 {
00034     while(KONDO2_serial.writeable())
00035     {
00036         KONDO2_serial.putc(KONDO2_Buffer_Tx[KONDO2_Buffer_Tx_Ptr]);
00037 //        KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
00038         KONDO2_Buffer_Tx_Ptr++;
00039  //       KONDO2_Buffer_Rx_Ptr++;
00040         if(KONDO2_Buffer_Tx_Ptr==3 && KONDO2_Buffer_Rx_Ptr==3)
00041               KONDO2_Buffer_Tx_Ptr = 0;
00042 //              KONDO2_Buffer_Rx_Ptr = 0;
00043     }
00044 }
00045 void KONDO2_Interrupt_Rx()
00046 {
00047     while(KONDO2_serial.readable())
00048     {
00049  //       KONDO2_Buffer_Rx_Ptr = 3;
00050         KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
00051         KONDO2_Buffer_Rx_Ptr++;
00052         if(KONDO2_Buffer_Rx_Ptr==6)
00053         {
00054             KONDO2_Buffer_Rx_Full = true;
00055             KONDO2_Buffer_Rx_Ptr=0;
00056         }
00057     }
00058 } 
00059 void KONDO2_update(unsigned char Id, unsigned short int Position)
00060 {
00061     unsigned char id,lo,hi;
00062     id=0x80|Id;
00063     hi=(Position>>7)&0x007F;
00064     lo=Position&0x007F;
00065     NVIC_DisableIRQ(USART2_IRQn);
00066     KONDO2_Buffer_Tx[0] = id;
00067     KONDO2_Buffer_Tx[1] = hi;
00068     KONDO2_Buffer_Tx[2] = lo;
00069 //    wait(0.0008);
00070     KONDO2_Buffer_Rx_Full = false;
00071     NVIC_EnableIRQ(USART2_IRQn);
00072 }
00073 void KONDO2_task() 
00074 {
00075     signed char id;
00076     signed char i;
00077     for(i=0 ; i < sizeof(KONDO2_SendOrder) ; i++)
00078     {
00079         id = KONDO2_SendOrder[i];
00080         KONDO2_update(id, KONDO2_Positions_Target[id]);
00081 //        wait_us(825);
00082         wait_us(650);
00083     }
00084 }
00085 void KONDO2_init() 
00086 {
00087     KONDO2_serial.format(8,Serial::Even,1);
00088     KONDO2_serial.baud(115200);
00089     KONDO2_Buffer_Rx_Ptr = 0;
00090     KONDO2_Buffer_Rx_Full = false;
00091     KONDO2_serial.attach(&KONDO2_Interrupt_Rx, Serial::RxIrq);
00092     KONDO2_serial.attach(&KONDO2_Interrupt_Tx, Serial::TxIrq);
00093     NVIC_EnableIRQ(USART2_IRQn);    
00094     KONDO2_queue.call_every(10, KONDO2_task);
00095     KONDO2_queue.dispatch();
00096 }