Lin ShengKun
/
NYP_FYP
FYP
Fork of humanoid by
kondo3.cpp
- Committer:
- ha731548874
- Date:
- 2018-05-24
- Revision:
- 4:45bd9ba6c0da
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(); }