| 128,15 → 128,18 |
|---|
| {-324000, 324000, 1200, 1600}, |
| {-324000, 324000, 1200, 1600}}}; |
| |
| //#define SERVO_TIMEOUT 10000 |
| |
| #define RXTX_BUFF_MAX 100 |
| volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX]; |
| volatile static BYTE *RXTX_addr[4][RXTX_BUFF_MAX]; |
| 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; |
| volatile unsigned int RX_cycle[4] = {0,0,0,0}; |
| volatile unsigned int TX_cycle[4] = {0,0,0,0}; |
| volatile unsigned int servo_skip[4] = {0,0,0,0}; |
| volatile unsigned int lock_write[4] = {0,0,0,0}; |
| volatile int servo_timer[4] = {NIL,NIL,NIL,NIL}; |
| |
| const int BaudTable[] = { |
| 1200, |
| 165,11 → 168,11 |
|---|
| RX_position[port]++; |
| |
| if (RX_position[port] >= RXTX_BUFF_MAX) { |
| RX_cycle++; |
| RX_cycle[port]++; |
| RX_position[port] = 0; |
| } |
| if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) || |
| (RX_cycle > TX_cycle)) |
| if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
| (RX_cycle[port] > TX_cycle[port])) |
| servo_rx_error(port,2); |
| |
| } else { |
| 179,11 → 182,11 |
|---|
| RX_position[port]++; |
| |
| if (RX_position[port] >= RXTX_BUFF_MAX) { |
| RX_cycle++; |
| RX_cycle[port]++; |
| RX_position[port] = 0; |
| } |
| if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) || |
| (RX_cycle > TX_cycle)) |
| if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
| (RX_cycle[port] > TX_cycle[port])) |
| servo_rx_error(port,3); |
| |
| } |
| 190,18 → 193,15 |
|---|
| |
| } |
| |
| extern int SCom_Error; |
| extern unsigned SCom_Error[4]; |
| |
| void servo_confirm(unsigned port, BYTE msg_status) { |
| |
| if (msg_status == COM_ERROR) { |
| kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error); |
| servo_skip[port] = 1; |
| kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error[port]); |
| } |
| |
| if (msg_status == COM_OK) { |
| lock_write[port] = 0; |
| } |
| lock_write[port] = 0; |
| |
| } |
| |
| 216,7 → 216,7 |
|---|
| f = kern_fsave(); |
| |
| old = TX_position[port]; |
| oldcycle = TX_cycle; |
| oldcycle = TX_cycle[port]; |
| |
| while(i < len_msg) { |
| RXTX_buff[port][TX_position[port]] = msg[i]; |
| 223,12 → 223,13 |
|---|
| RXTX_addr[port][TX_position[port]] = NULL; |
| TX_position[port]++; |
| if (TX_position[port] >= RXTX_BUFF_MAX) { |
| TX_cycle++; |
| TX_cycle[port]++; |
| TX_position[port] = 0; |
| } |
| if (TX_position[port] == RX_position[port]) { |
| TX_position[port] = old; |
| TX_cycle = oldcycle; |
| TX_cycle[port] = oldcycle; |
| lock_write[port] = 0; |
| kern_frestore(f); |
| return -1; |
| } |
| 242,12 → 243,13 |
|---|
| RX = TX_position[port]; |
| TX_position[port]++; |
| if (TX_position[port] >= RXTX_BUFF_MAX) { |
| TX_cycle++; |
| TX_cycle[port]++; |
| TX_position[port] = 0; |
| } |
| if (TX_position[port] == RX_position[port]) { |
| TX_position[port] = old; |
| TX_cycle = oldcycle; |
| TX_cycle[port] = oldcycle; |
| lock_write[port] = 0; |
| kern_frestore(f); |
| return -1; |
| } |
| 262,13 → 264,38 |
|---|
| |
| } |
| |
| void servo_timeout(void *arg) { |
| |
| unsigned port = (unsigned)(arg); |
| |
| servo_timer[port] = NIL; |
| servo_skip[port] = 1; |
| |
| kern_printf("(SERVO: TIMEOUT PORT:%d)",port); |
| |
| } |
| |
| int servo_wait(unsigned port, int RX) { |
| |
| #ifdef SERVO_TIMEOUT |
| struct timespec acttime; |
| #endif |
| servo_skip[port] = 0; |
| |
| #ifdef SERVO_TIMEOUT |
| kern_gettime(&acttime); |
| ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&acttime); |
| servo_timer[port] = kern_event_post(&acttime,servo_timeout,(void *)(port)); |
| #endif |
| |
| /* Active wait until number bytes received */ |
| while (RXTX_buff[port][RX] == 0 && servo_skip[port] == 0); |
| |
| #ifdef SERVO_TIMEOUT |
| if (servo_timer[port] != NIL) |
| kern_event_delete(servo_timer[port]); |
| #endif |
| |
| return 0; |
| |
| } |
| 594,7 → 621,7 |
|---|
| int servo_set_angle_sec(int port, int servo, int angle_sec) |
| { |
| unsigned char b[3]; |
| int angle_tick; |
| int angle_tick, res = 0; |
| int servo_port = (unsigned)(port); |
| |
| if (servo > 15) return -1; |
| 606,9 → 633,9 |
|---|
| b[0] = 0x00 | (servo & 0x0F); |
| b[1] = 0x00 | ((angle_tick >> 8) & 0x0F); |
| b[2] = 0x00 | (angle_tick & 0xFF); |
| servo_send_msg(servo_port, b, 3, NULL, 0); |
| res = servo_send_msg(servo_port, b, 3, NULL, 0); |
| |
| return 0; |
| return res; |
| |
| } |
| |
| 616,14 → 643,15 |
|---|
| int servo_store_default_position(int port, int servo) |
| { |
| unsigned char b[1]; |
| int res = 0; |
| int servo_port = (unsigned)(port); |
| |
| if (servo > 15) return -1; |
| |
| b[0] = 0x20 | (servo & 0x0F); |
| servo_send_msg(servo_port, b, 1, NULL, 0); |
| res = servo_send_msg(servo_port, b, 1, NULL, 0); |
| |
| return 0; |
| return res; |
| |
| } |
| |
| 639,17 → 667,25 |
|---|
| b[0] = 0x10 | (servo & 0x0F); |
| RX = servo_send_msg(servo_port, b, 1, r, 2); |
| |
| servo_wait(servo_port, RX); |
| if (RX != -1) { |
| |
| res = (int)(r[0]) << 8; |
| res |= r[1]; |
| servo_wait(servo_port, RX); |
| |
| data = ((res*TICK_LEN/1000) - servo_table[port][servo].zero_tick) * |
| (servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) / |
| servo_table[port][servo].delta_tick; |
| if (servo_skip[port] == 0) { |
| |
| return data; |
| res = (int)(r[0]) << 8; |
| res |= r[1]; |
| |
| data = ((res*TICK_LEN/1000) - servo_table[port][servo].zero_tick) * |
| (servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) / |
| servo_table[port][servo].delta_tick; |
| |
| return data; |
| |
| } else return -1; |
| |
| } else return -1; |
| |
| } |
| |
| /* 0100:0aaa */ |