Lin ShengKun
/
NYP_FYP
FYP
Fork of humanoid by
Diff: kondo2.cpp
- Revision:
- 4:45bd9ba6c0da
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/kondo2.cpp Thu May 24 08:29:52 2018 +0000 @@ -0,0 +1,96 @@ +#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(); +}