506,14 → 506,16 |
|
} |
|
/* 1000.0000:0000.Mmmm */ |
/* 1000.0000:0000.0mmm */ |
int servo_turn_off(int servo) |
{ |
|
struct timespec current_time; |
unsigned char b; |
int err; |
if (servo > 15) return -1; |
int err; |
|
if (servo > 7) return -1; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
524,7 → 526,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | (servo & 0x0F); |
b = 0x00 | (servo & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
546,7 → 548,7 |
unsigned char b; |
int err; |
|
if (servo > 15) return -1; |
if (servo > 7) return -1; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
558,7 → 560,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x10 | (servo & 0x0F); |
b = 0x10 | (servo & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
574,8 → 576,7 |
|
/* 1000.0000:0010.0000 */ |
int servo_turn_off_all(void) |
{ |
struct timespec current_time; |
{ struct timespec current_time; |
unsigned char b; |
int err; |
|
634,8 → 635,8 |
|
} |
|
/* 1000.0000:0101.000M:mmmm.mmmm */ |
int servo_set_levels(int bank,int mask) |
/* 1000.0000:0011.0000:mmmm.mmmm */ |
int servo_set_levels(int mask) |
{ |
struct timespec current_time; |
unsigned char b; |
651,7 → 652,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x50 | (0x01 & bank); |
b = 0x30; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
670,8 → 671,8 |
|
} |
|
/* 1000.0000:0100.000M */ |
int servo_get_levels(int bank) |
/* 1000.0000:0100.0000 */ |
int servo_get_levels(void) |
{ |
struct timespec current_time; |
unsigned char b; |
687,7 → 688,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x40 | (0x01 & bank); |
b = 0x40; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
749,7 → 750,7 |
|
} |
|
/* 0000.Pppp:0000.vvvv:vvvv.vvvv */ |
/* 0000.0ppp:0000.vvvv:vvvv.vvvv */ |
int servo_set_angle_sec(int servo, int angle_sec) |
{ |
struct timespec current_time; |
756,7 → 757,7 |
unsigned char b; |
int err, angle_tick; |
|
if (servo > 15) return -1; |
if (servo > 7) return -1; |
|
if (angle_sec > servo_table[servo].max_angle_sec || |
angle_sec < servo_table[servo].min_angle_sec) return -1; |
769,7 → 770,7 |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x00 | (servo & 0x0F); |
b = 0x00 | (servo & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
793,7 → 794,7 |
|
} |
|
/* 0010.Pppp */ |
/* 0010.0ppp */ |
int servo_store_default_position(int servo) |
{ |
struct timespec current_time; |
800,7 → 801,7 |
unsigned char b; |
int err; |
|
if (servo > 15) return -1; |
if (servo > 7) return -1; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
807,7 → 808,7 |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x20 | (servo & 0x0F); |
b = 0x20 | (servo & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
821,7 → 822,7 |
|
} |
|
/* 0001.Pppp */ |
/* 0001.0ppp */ |
int servo_get_angle_sec(int servo) |
{ |
struct timespec current_time; |
828,7 → 829,7 |
unsigned char b; |
int err,res,data; |
|
if (servo > 15) return -1; |
if (servo > 7) return -1; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
835,7 → 836,7 |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x10 | (servo & 0x0F); |
b = 0x10 | (servo & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
863,7 → 864,7 |
unsigned char b; |
int err,res; |
|
if (port > 7) return -1; |
if (port > 4) return -1; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |