61,6 → 61,7 |
|
#define SERVO_TIMEOUT 100000 /* us */ |
|
#define SERVO_PORT 2 |
#define SERVO_SPEED 38400 |
#define SERVO_PARITY NONE |
#define SERVO_LEN 8 |
68,7 → 69,6 |
|
int timer_expired = 0; |
int timeout_event; |
int servo_port; |
|
const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
|
195,12 → 195,11 |
|
} |
|
int servo_open(int port) |
int servo_open(void) |
{ |
int err; |
|
servo_port = (unsigned)(port); |
err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
err = com_open(SERVO_PORT, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
|
return err; |
|
210,7 → 209,7 |
{ |
int err; |
|
err = com_close(servo_port); |
err = com_close(SERVO_PORT); |
|
return err; |
|
261,19 → 260,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; |
295,10 → 294,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); |
|
322,8 → 321,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); |
335,7 → 334,7 |
|
} |
|
/* 1000.1010:llll.llll */ |
/* 1000.1000:llll.llll */ |
int servo_set_period(int period) |
{ |
struct timespec current_time; |
347,14 → 346,14 |
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; |
|
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); |
379,10 → 378,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); |
|
393,7 → 392,7 |
|
} |
|
/* 1000.1000 */ |
/* 1000.1010 */ |
int servo_store_period(void) |
{ |
struct timespec current_time; |
405,9 → 404,9 |
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; |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
431,10 → 430,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); |
|
458,8 → 457,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); |
487,13 → 486,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); |
521,13 → 520,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); |
551,13 → 550,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); |
582,13 → 581,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); |
615,18 → 614,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); |
653,11 → 652,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); |
|
684,11 → 683,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); |
|