377,22 → 377,30 |
b[0] = 0x85; |
RX = servo_send_msg(servo_port, b, 1, r, 2); |
|
servo_wait(servo_port, RX); |
if (RX != -1) { |
|
servo_wait(servo_port, RX); |
|
if (servo_skip[port] == 0) { |
|
res_w = r[0]; /* bit W */ |
res_b = r[1]; /* byte SPBRG */ |
res_w = r[0]; /* bit W */ |
res_b = r[1]; /* byte SPBRG */ |
|
if (res_w != -1 && res_b != -1) { |
if (res_w) |
res = SERVO_CLOCK / ( 16 * (res_b + 1) ); |
else |
res = SERVO_CLOCK / ( 64 * (res_b + 1) ); |
} else { |
return -1; |
} |
if (res_w != -1 && res_b != -1) { |
if (res_w) |
res = SERVO_CLOCK / ( 16 * (res_b + 1) ); |
else |
res = SERVO_CLOCK / ( 64 * (res_b + 1) ); |
} else { |
return -1; |
} |
|
return res; |
return res; |
|
} else return -1; |
|
} else return -1; |
|
} |
|
/* 1000.0100 */ |
432,12 → 440,20 |
b[0] = 0x89; |
RX = servo_send_msg(servo_port, b, 1, r, 1); |
|
servo_wait(servo_port, RX); |
if (RX != -1) { |
|
servo_wait(servo_port, RX); |
|
if (servo_skip[port] == 0) { |
|
res = r[0]; |
res = r[0]; |
|
return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
|
} else return -1; |
|
} else return -1; |
|
} |
|
/* 1000.1000 */ |
463,12 → 479,20 |
b[0] = 0x8C; |
RX = servo_send_msg(servo_port, b, 1, r, 1); |
|
servo_wait(servo_port, RX); |
if (RX != -1) { |
|
servo_wait(servo_port, RX); |
|
if (servo_skip[port] == 0) { |
|
res = r[0]; |
res = r[0]; |
|
return res; |
return res; |
|
} else return -1; |
|
} else return -1; |
|
} |
|
/* 1000.111s */ |
569,13 → 593,21 |
b[0] = 0x80; |
b[1] = 0x40 | (0x01 & bank); |
RX = servo_send_msg(servo_port, b, 2, r, 1); |
|
servo_wait(servo_port, RX); |
|
res = r[0]; |
if (RX != -1) { |
|
servo_wait(servo_port, RX); |
|
if (servo_skip[port] == 0) { |
|
return res; |
res = r[0]; |
|
return res; |
|
} else return -1; |
|
} else return -1; |
|
} |
|
/* 1000.0000:1000.0000 */ |
700,12 → 732,20 |
b[0] = 0x40 | (adport & 0x07); |
RX = servo_send_msg(servo_port, b, 1, r, 2); |
|
servo_wait(servo_port, RX); |
if (RX != -1) { |
|
res = r[0] << 8; |
res |= r[1]; |
servo_wait(servo_port, RX); |
|
return res; |
if (servo_skip[port] == 0) { |
|
res = (int)r[0] << 8; |
res |= r[1]; |
|
return res; |
|
} else return -1; |
|
} else return -1; |
|
} |
|