377,8 → 377,12 |
b[0] = 0x85; |
RX = servo_send_msg(servo_port, b, 1, r, 2); |
|
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 */ |
|
393,6 → 397,10 |
|
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); |
|
if (RX != -1) { |
|
servo_wait(servo_port, RX); |
|
if (servo_skip[port] == 0) { |
|
res = r[0]; |
|
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); |
|
if (RX != -1) { |
|
servo_wait(servo_port, RX); |
|
if (servo_skip[port] == 0) { |
|
res = r[0]; |
|
return res; |
|
} else return -1; |
|
} else return -1; |
|
} |
|
/* 1000.111s */ |
570,12 → 594,20 |
b[1] = 0x40 | (0x01 & bank); |
RX = servo_send_msg(servo_port, b, 2, r, 1); |
|
if (RX != -1) { |
|
servo_wait(servo_port, RX); |
|
if (servo_skip[port] == 0) { |
|
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); |
|
if (RX != -1) { |
|
servo_wait(servo_port, RX); |
|
res = r[0] << 8; |
if (servo_skip[port] == 0) { |
|
res = (int)r[0] << 8; |
res |= r[1]; |
|
return res; |
|
} else return -1; |
|
} else return -1; |
|
} |
|