103,47 → 103,172 |
} |
} |
|
/* 1000.0100:bbbb.bbbb */ |
int servo_set_RS232_baudrate(int baud) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
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; |
|
|
return 0; |
b = baud & 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.0101 */ |
int servo_get_RS232_baudrate(void) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,res; |
|
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); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return res; |
else |
return -1; |
|
return 0; |
|
} |
|
/* 1000.0110 */ |
int servo_store_RS232_buadrate(void) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
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; |
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 0; |
|
} |
|
/* 1000.1000:llll.llll */ |
int servo_set_period(int period) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
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; |
|
b = 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 |
return -1; |
|
return 0; |
|
} |
|
/* 1000.1001 */ |
int servo_get_period(void) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,res; |
|
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); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return res*8; |
else |
return -1; |
|
return 0; |
|
} |
|
/* 1000.1010 */ |
int servo_store_period(void) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
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; |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
return 0; |
|
} |
|
/* 1000.1100 */ |
198,10 → 323,138 |
|
} |
|
/* 1000.0000:0000.0mmm */ |
int servo_turn_off(int servo) |
{ |
|
struct timespec current_time; |
unsigned char b; |
int err; |
|
if (servo > 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 = 0x80; |
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); |
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:1000.0mmm */ |
int servo_turn_on(int servo) |
{ |
|
struct timespec current_time; |
unsigned char b; |
int err; |
|
if (servo > 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 = 0x80; |
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); |
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:0001.0000 */ |
int servo_turn_off_all(void) |
{ struct timespec current_time; |
unsigned char b; |
int err; |
|
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; |
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:0001.0001 */ |
int servo_turn_on_all(void) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
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 = 0x11; |
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; |
|
} |
|
/* 0000.0ppp:0000.vvvv:vvvv.vvvv */ |
int servo_set_angle(int servo, int angle) |
{ |
|
struct timespec current_time; |
unsigned char b; |
int err; |