57,9 → 57,9 |
|
#define barrier() __asm__ __volatile__("" ::: "memory"); |
|
#define SERVO_DEBUG |
//#define SERVO_DEBUG |
|
#define SERVO_TIMEOUT 100000 /* us */ |
#define SERVO_TIMEOUT 200000 /* us */ |
|
#define SERVO_SPEED 38400 |
#define SERVO_PARITY NONE |
66,6 → 66,8 |
#define SERVO_LEN 8 |
#define SERVO_STOP 1 |
|
#define SERVO_CLOCK 20000000 /* 20MHz */ |
|
#define TICK_LEN 1600 /* ns */ |
#define TICK_LEN_PERIOD 51200 /* ns */ |
|
96,7 → 98,6 |
|
int timer_expired = 0; |
int timeout_event; |
int servo_port; |
|
const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
|
112,37 → 113,6 |
115200, |
-1}; |
|
/* 20MHz: SPBRG, BAUD BRGH=0, BAUD BRGH=1 */ |
const int Baud20MhzData[] = { |
255, 1221, 4882, |
254, 1225, 4902, |
253, 1230, 4921, |
252, 1235, 4941, |
133, 2332, 9328, |
132, 2350, 9398, |
131, 2367, 9470, |
130, 2385, 9542, |
129, 2404, 9615, |
128, 2422, 9690, |
127, 2441, 9766, |
126, 2461, 9842, |
88, 0, 14045, |
87, 0, 14204, |
86, 0, 14368, |
85, 0, 14535, |
84, 0, 14706, |
66, 4664, 18657, |
65, 4735, 18939, |
64, 4808, 19231, |
63, 4883, 19531, |
32, 9470, 37879, |
31, 9766, 39062, |
21, 14205, 56818, |
15, 19531, 0, |
10, 0, 113636, |
7, 39062, 0, |
-1, -1, -1}; |
|
void set_timer_expired(void *arg) |
{ |
timeout_event = NIL; |
238,22 → 208,21 |
|
} |
|
int servo_open(int port) |
int servo_open(int port,int speed) |
{ |
int err; |
|
servo_port = (unsigned)(port); |
err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
|
err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
|
return err; |
|
} |
|
int servo_close(void) |
int servo_close(int port) |
{ |
int err; |
|
err = com_close(servo_port); |
err = com_close((unsigned)(port)); |
|
return err; |
|
260,12 → 229,13 |
} |
|
/* 1000.011w:bbbb.bbbb */ |
int servo_set_RS232_baudrate(int baud) |
int servo_set_RS232_baudrate(int port, int baud) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,diff,mindiff,i; |
int err, spbrg_temp, i; |
unsigned char spbrg, w; |
int servo_port = (unsigned)(port); |
|
i = 0; |
while(BaudTable[i] != baud && BaudTable[i] != -1) i++; |
274,35 → 244,24 |
return -1; |
} |
|
i = 0; |
mindiff = 0xFFFFFF; |
spbrg = 0; |
w = 0; |
while(Baud20MhzData[i] != -1) { |
diff = abs(Baud20MhzData[i+1] - baud); |
if (diff < mindiff) { |
mindiff = diff; |
spbrg = (unsigned char)(Baud20MhzData[i]); |
w = 0; |
} |
diff = abs(Baud20MhzData[i+2] - baud); |
if (diff < mindiff) { |
mindiff = diff; |
spbrg = (unsigned char)(Baud20MhzData[i]); |
w = 1; |
} |
i += 3; |
w = 1; |
spbrg_temp = (SERVO_CLOCK / (16*baud)) - 1; |
if (spbrg_temp>255) { |
w = 0; |
spbrg = (SERVO_CLOCK / (64*baud)) - 1; |
} else { |
spbrg = spbrg_temp; |
} |
|
|
#ifdef SERVO_DEBUG |
kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w); |
#endif |
|
#endif |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x86 | (w & 0x01); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
314,10 → 273,10 |
if (err != (int)(b)) timer_expired = 1; |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
com_close(servo_port); |
com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
|
|
/*com_close(servo_port); |
com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);*/ |
|
if (!timer_expired) |
return 0; |
else |
326,25 → 285,31 |
} |
|
/* 1000.0101 */ |
int servo_get_RS232_baudrate(void) |
int servo_get_RS232_baudrate(int port) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,res; |
|
int err, res, res_w, res_b; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x85; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(servo_port); |
|
res_w = com_receive(servo_port); /* bit W */ |
res_b = com_receive(servo_port); /* byte SPBRG */ |
if (res_w) |
res = SERVO_CLOCK / ( 16 * (res_b + 1) ); |
else |
res = SERVO_CLOCK / ( 64 * (res_b + 1) ); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return res; |
else |
353,55 → 318,57 |
} |
|
/* 1000.0100 */ |
int servo_store_RS232_baudrate(void) |
int servo_store_RS232_baudrate(int port) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x84; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
return -1; |
return -1; |
|
} |
|
/* 1000.1010:llll.llll */ |
int servo_set_period(int period) |
int servo_set_period(int port, int period) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x8A; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
b = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
410,25 → 377,26 |
} |
|
/* 1000.1001 */ |
int servo_get_period(void) |
int servo_get_period(int port) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,res; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x89; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(servo_port); |
|
res = com_receive(servo_port); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
else |
437,41 → 405,44 |
} |
|
/* 1000.1000 */ |
int servo_store_period(void) |
int servo_store_period(int port) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x88; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
return -1; |
return -1; |
|
} |
|
/* 1000.1100 */ |
int servo_get_setup_switch(void) |
int servo_get_setup_switch(int port) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,res; |
int err,res; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x8C; |
err = com_send(servo_port, b); |
489,12 → 460,13 |
} |
|
/* 1000.111s */ |
int servo_set_RC5_switch(int data) |
int servo_set_RC5_switch(int port, int data) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
505,8 → 477,8 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return 0; |
else |
515,13 → 487,15 |
} |
|
/* 1000.0000:0000.Mmmm */ |
int servo_turn_off(int servo) |
int servo_turn_off(int port, int servo) |
{ |
|
struct timespec current_time; |
unsigned char b; |
int err; |
if (servo > 15) return -1; |
int err; |
int servo_port = (unsigned)(port); |
|
if (servo > 15) return -1; |
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
538,7 → 512,7 |
if (err != (int)(b)) timer_expired = 1; |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
547,201 → 521,207 |
} |
|
/* 1000.0000:0001.Mmmm */ |
int servo_turn_on(int servo) |
int servo_turn_on(int port, int servo) |
{ |
|
|
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
if (servo > 15) return -1; |
|
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
b = 0x10 | (servo & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
|
} |
|
/* 1000.0000:0010.0000 */ |
int servo_turn_off_all(void) |
int servo_turn_off_all(int port) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
b = 0x20; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
|
} |
|
/* 1000.0000:0010.0001 */ |
int servo_turn_on_all(void) |
int servo_turn_on_all(int port) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
b = 0x21; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
|
} |
|
/* 1000.0000:0101.000M:mmmm.mmmm */ |
int servo_set_levels(int bank,int mask) |
int servo_set_levels(int port, int bank,int mask) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
b = 0x50 | (0x01 & bank); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
b = (unsigned char)(mask & 0xFF); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
|
} |
|
/* 1000.0000:0100.000M */ |
int servo_get_levels(int bank) |
int servo_get_levels(int port, int bank) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
b = 0x40 | (0x01 & bank); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
err = com_receive(servo_port); |
|
err = com_receive(servo_port); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return err; |
else |
return -1; |
|
|
} |
|
/* 1000.0000:1000.0000 */ |
int servo_store_levels(void) |
int servo_store_levels(int port) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
|
} |
|
int servo_set_max_angle(int servo, int angle_sec) |
int servo_set_max_angle(int port, int servo, int angle_sec) |
{ |
|
servo_table[servo].max_angle_sec = angle_sec; |
758,18 → 738,19 |
} |
|
/* 0000.Pppp:0000.vvvv:vvvv.vvvv */ |
int servo_set_angle_sec(int servo, int angle_sec) |
int servo_set_angle_sec(int port, int servo, int angle_sec) |
{ |
struct timespec current_time; |
unsigned char b; |
int err, angle_tick; |
int servo_port = (unsigned)(port); |
|
if (servo > 15) return -1; |
|
if (angle_sec > servo_table[servo].max_angle_sec || |
if (angle_sec > servo_table[servo].max_angle_sec || |
angle_sec < servo_table[servo].min_angle_sec) return -1; |
|
angle_tick = (servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick / |
angle_tick = (servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick / |
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec)) * 1000 / TICK_LEN; |
|
timer_expired = 0; |
776,7 → 757,7 |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x00 | (servo & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
790,7 → 771,7 |
b = 0x00 | (angle_tick & 0xFF); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
if (err != (int)(b)) timer_expired = 1; |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
802,26 → 783,27 |
} |
|
/* 0010.Pppp */ |
int servo_store_default_position(int servo) |
int servo_store_default_position(int port, int servo) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
if (servo > 15) return -1; |
|
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
|
b = 0x20 | (servo & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
|
if (!timer_expired) |
return 0; |
else |
830,14 → 812,15 |
} |
|
/* 0001.Pppp */ |
int servo_get_angle_sec(int servo) |
int servo_get_angle_sec(int port, int servo) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,res,data; |
|
int servo_port = (unsigned)(port); |
|
if (servo > 15) return -1; |
|
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
852,7 → 835,7 |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
data = ((res*TICK_LEN/1000) - servo_table[servo].zero_tick) * |
data = ((res*TICK_LEN/1000) - servo_table[servo].zero_tick) * |
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) / |
servo_table[servo].delta_tick; |
|
864,21 → 847,22 |
} |
|
/* 0100:0aaa */ |
int servo_get_analog(int port) |
int servo_get_analog(int port, int adport) |
{ |
|
struct timespec current_time; |
unsigned char b; |
int err,res; |
|
int servo_port = (unsigned)(port); |
|
if (port > 7) return -1; |
|
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x40 | (port & 0x07); |
b = 0x40 | (adport & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |