61,7 → 61,6 |
|
#define SERVO_TIMEOUT 100000 /* us */ |
|
#define SERVO_PORT 2 |
#define SERVO_SPEED 38400 |
#define SERVO_PARITY NONE |
#define SERVO_LEN 8 |
69,6 → 68,7 |
|
int timer_expired = 0; |
int timeout_event; |
int servo_port; |
|
const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
|
195,11 → 195,12 |
|
} |
|
int servo_open(void) |
int servo_open(int port) |
{ |
int err; |
|
err = com_open(SERVO_PORT, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
servo_port = (unsigned)(port); |
err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
|
return err; |
|
209,7 → 210,7 |
{ |
int err; |
|
err = com_close(SERVO_PORT); |
err = com_close(servo_port); |
|
return err; |
|
260,19 → 261,19 |
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); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = spbrg; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
|
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; |
294,10 → 295,10 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x85; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
|
321,8 → 322,8 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x84; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
334,7 → 335,7 |
|
} |
|
/* 1000.1000:llll.llll */ |
/* 1000.1010:llll.llll */ |
int servo_set_period(int period) |
{ |
struct timespec current_time; |
346,14 → 347,14 |
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); |
b = 0x8A; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = period/8 & 0xFF; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
378,10 → 379,10 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x89; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
|
392,7 → 393,7 |
|
} |
|
/* 1000.1010 */ |
/* 1000.1000 */ |
int servo_store_period(void) |
{ |
struct timespec current_time; |
404,9 → 405,9 |
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); |
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); |
430,10 → 431,10 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x8C; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
|
457,8 → 458,8 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x8E | (data & 0x01); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
486,13 → 487,13 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x80; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | (servo & 0x07); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
520,13 → 521,13 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x80; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x08 | (servo & 0x07); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
550,13 → 551,13 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x80; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x10; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
581,13 → 582,13 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x80; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x11; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
614,18 → 615,18 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x00 | (servo & 0x07); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | ((angle >> 8) & 0x0F); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | (angle & 0xFF); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
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); |
652,11 → 653,11 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x08 | (servo & 0x07); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(SERVO_PORT) << 8; |
res |= com_receive(SERVO_PORT); |
res = com_receive(servo_port) << 8; |
res |= com_receive(servo_port); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
683,11 → 684,11 |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x40 | (port & 7); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(SERVO_PORT) << 8; |
res |= com_receive(SERVO_PORT); |
res = com_receive(servo_port) << 8; |
res |= com_receive(servo_port); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|