135,8 → 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; |
volatile unsigned int servo_skip[4] = {0,0,0,0}; |
volatile unsigned int lock_write[4] = {0,0,0,0}; |
|
const int BaudTable[] = { |
1200, |
196,11 → 196,11 |
|
if (msg_status == COM_ERROR) { |
kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error); |
servo_skip = 1; |
servo_skip[port] = 1; |
} |
|
if (msg_status == COM_OK) { |
lock_write = 0; |
lock_write[port] = 0; |
} |
|
} |
210,8 → 210,8 |
int i = 0, old, oldcycle, RX = 0; |
SYS_FLAGS f; |
|
while(lock_write == 1); |
lock_write = 1; |
while(lock_write[port] == 1); |
lock_write[port] = 1; |
|
f = kern_fsave(); |
|
264,10 → 264,10 |
|
int servo_wait(unsigned port, int RX) { |
|
servo_skip = 0; |
servo_skip[port] = 0; |
|
/* Active wait until number bytes received */ |
while (RXTX_buff[port][RX] == 0 && servo_skip == 0); |
while (RXTX_buff[port][RX] == 0 && servo_skip[port] == 0); |
|
return 0; |
|