| 135,6 → 135,8 |
|---|
| volatile unsigned int TX_position[4] = {0,0,0,0}; |
| volatile unsigned int RX_cycle = 0; |
| volatile unsigned int TX_cycle = 0; |
| volatile unsigned int servo_skip = 0; |
| volatile unsigned int lock_write = 0; |
| |
| const int BaudTable[] = { |
| 1200, |
| 182,24 → 184,35 |
|---|
| } |
| if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) || |
| (RX_cycle > TX_cycle)) |
| servo_rx_error(port,2); |
| servo_rx_error(port,3); |
| |
| } |
| |
| } |
| |
| extern int SCom_Error; |
| |
| void servo_confirm(unsigned port, BYTE msg_status) { |
| |
| if (msg_status == COM_ERROR) |
| kern_printf("(SERVO: PORT:%d ERROR)",port); |
| if (msg_status == COM_ERROR) { |
| kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error); |
| servo_skip = 1; |
| } |
| |
| if (msg_status == COM_OK) { |
| lock_write = 0; |
| } |
| |
| } |
| |
| int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) { |
| |
| int i = 0, old, oldcycle, RX = 0; |
| SYS_FLAGS f; |
| |
| while(lock_write == 1); |
| lock_write = 1; |
| |
| f = kern_fsave(); |
| |
| old = TX_position[port]; |
| 251,8 → 264,10 |
|---|
| |
| int servo_wait(unsigned port, int RX) { |
| |
| servo_skip = 0; |
| |
| /* Active wait until number bytes received */ |
| while (RXTX_buff[port][RX] == 0); |
| while (RXTX_buff[port][RX] == 0 && servo_skip == 0); |
| |
| return 0; |
| |