| 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; |
| |
| } |
| |