| 131,10 → 131,10 |
|---|
| #define RXTX_BUFF_MAX 100 |
| static BYTE RXTX_buff[4][RXTX_BUFF_MAX]; |
| static BYTE *RXTX_addr[4][RXTX_BUFF_MAX]; |
| unsigned int RX_position[4] = {0,0,0,0}; |
| unsigned int TX_position[4] = {0,0,0,0}; |
| unsigned int RX_cycle = 0; |
| unsigned int TX_cycle = 0; |
| volatile unsigned int RX_position[4] = {0,0,0,0}; |
| volatile unsigned int TX_position[4] = {0,0,0,0}; |
| volatile unsigned int RX_cycle = 0; |
| volatile unsigned int TX_cycle = 0; |
| |
| const int BaudTable[] = { |
| 1200, |
| 193,7 → 193,7 |
|---|
| |
| int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) { |
| |
| int i = 0, old, oldcycle; |
| int i = 0, old, oldcycle, RX; |
| SYS_FLAGS f; |
| |
| f = kern_fsave(); |
| 236,10 → 236,29 |
|---|
| i++; |
| } |
| |
| RX = (int)RX_position; |
| |
| kern_frestore(f); |
| |
| com_irq_send(port, len_msg, msg); |
| |
| return RX; |
| |
| } |
| |
| int servo_wait(unsigned port, int RX, int number) { |
| |
| int f = RX, i = 0; |
| |
| while (i < number) { |
| f++; |
| if (f >= RXTX_BUFF_MAX) f = 0; |
| i++; |
| } |
| |
| /* Active wait until number bytes received */ |
| while (RX_position[port] < f); |
| |
| return 0; |
| |
| } |
| 316,10 → 335,13 |
|---|
| unsigned char b[1],r[2]; |
| int res, res_w, res_b; |
| int servo_port = (unsigned)(port); |
| int RX; |
| |
| b[0] = 0x85; |
| servo_send_msg(servo_port, b, 1, r, 2); |
| RX = servo_send_msg(servo_port, b, 1, r, 2); |
| |
| servo_wait(servo_port, RX, 2); |
| |
| res_w = r[0]; /* bit W */ |
| res_b = r[1]; /* byte SPBRG */ |
| |
| 367,12 → 389,14 |
|---|
| int servo_get_period(int port) |
| { |
| unsigned char b[1],r[1]; |
| int res; |
| int res, RX; |
| int servo_port = (unsigned)(port); |
| |
| b[0] = 0x89; |
| servo_send_msg(servo_port, b, 1, r, 1); |
| RX = servo_send_msg(servo_port, b, 1, r, 1); |
| |
| servo_wait(servo_port, RX, 1); |
| |
| res = r[0]; |
| |
| return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
| 396,12 → 420,14 |
|---|
| int servo_get_setup_switch(int port) |
| { |
| unsigned char b[1],r[1]; |
| int res; |
| int res, RX; |
| int servo_port = (unsigned)(port); |
| |
| b[0] = 0x8C; |
| servo_send_msg(servo_port, b, 1, r, 1); |
| RX = servo_send_msg(servo_port, b, 1, r, 1); |
| |
| servo_wait(servo_port, RX, 1); |
| |
| res = r[0]; |
| |
| return res; |
| 500,13 → 526,15 |
|---|
| int servo_get_levels(int port, int bank) |
| { |
| unsigned char b[2],r[1]; |
| int res; |
| int res, RX; |
| int servo_port = (unsigned)(port); |
| |
| b[0] = 0x80; |
| b[1] = 0x40 | (0x01 & bank); |
| servo_send_msg(servo_port, b, 2, r, 1); |
| RX = servo_send_msg(servo_port, b, 2, r, 1); |
| |
| servo_wait(servo_port, RX, 1); |
| |
| res = r[0]; |
| |
| return res; |
| 593,14 → 621,16 |
|---|
| int servo_get_angle_sec(int port, int servo) |
| { |
| unsigned char b[1],r[2]; |
| int res,data; |
| int res,data,RX; |
| int servo_port = (unsigned)(port); |
| |
| if (servo > 15) return -1; |
| |
| b[0] = 0x10 | (servo & 0x0F); |
| servo_send_msg(servo_port, b, 1, r, 2); |
| RX = servo_send_msg(servo_port, b, 1, r, 2); |
| |
| servo_wait(servo_port, RX, 2); |
| |
| res = r[0] << 8; |
| res |= r[1]; |
| |
| 616,14 → 646,16 |
|---|
| int servo_get_analog(int port, int adport) |
| { |
| unsigned char b[1],r[2]; |
| int res; |
| int res, RX; |
| int servo_port = (unsigned)(port); |
| |
| if (adport > 7) return -1; |
| |
| b[0] = 0x40 | (adport & 0x07); |
| servo_send_msg(servo_port, b, 1, r, 2); |
| RX = servo_send_msg(servo_port, b, 1, r, 2); |
| |
| servo_wait(servo_port, RX, 2); |
| |
| res = r[0] << 8; |
| res |= r[1]; |
| |