524,7 → 524,7 |
|
} |
|
/* 1000.0000:1000.0mmm */ |
/* 1000.0000:0001.0mmm */ |
int servo_turn_on(int servo) |
{ |
|
544,7 → 544,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x08 | (servo & 0x07); |
b = 0x10 | (servo & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
558,7 → 558,7 |
|
} |
|
/* 1000.0000:0001.0000 */ |
/* 1000.0000:0010.0000 */ |
int servo_turn_off_all(void) |
{ struct timespec current_time; |
unsigned char b; |
574,7 → 574,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x10; |
b = 0x20; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
588,7 → 588,7 |
|
} |
|
/* 1000.0000:0001.0001 */ |
/* 1000.0000:0010.0001 */ |
int servo_turn_on_all(void) |
{ |
struct timespec current_time; |
605,7 → 605,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x11; |
b = 0x21; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
679,7 → 679,7 |
|
} |
|
/* 0001.0ppp */ |
/* 0010.0ppp */ |
int servo_store_default_position(int servo) |
{ |
struct timespec current_time; |
693,7 → 693,7 |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x10 | (servo & 0x07); |
b = 0x20 | (servo & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
707,7 → 707,7 |
|
} |
|
/* 0000.1ppp */ |
/* 0001.0ppp */ |
int servo_get_angle_sec(int servo) |
{ |
struct timespec current_time; |
721,7 → 721,7 |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x08 | (servo & 0x07); |
b = 0x10 | (servo & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |