66,6 → 66,25 |
#define SERVO_LEN 8 |
#define SERVO_STOP 1 |
|
#define TICK_LEN 1600 /* us */ |
|
struct servo_data { |
int min_angle_sec; |
int max_angle_sec; |
int delta_tick; |
int zero_tick; |
}; |
|
struct servo_data servo_table[] = { |
{-324000, 324000, 1800, 1600}, |
{-324000, 324000, 1800, 1600}, |
{-324000, 324000, 1800, 1600}, |
{-324000, 324000, 1800, 1600}, |
{-324000, 324000, 1800, 1600}, |
{-324000, 324000, 1800, 1600}, |
{-324000, 324000, 1800, 1600}, |
{-324000, 324000, 1800, 1600}}; |
|
int timer_expired = 0; |
int timeout_event; |
int servo_port; |
352,7 → 371,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = period/8 & 0xFF; |
b = period/TICK_LEN/8 & 0xFF; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
387,7 → 406,7 |
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return res*8; |
return (res*TICK_LEN*8); |
else |
return -1; |
|
600,15 → 619,37 |
|
} |
|
int servo_set_max_angle(int servo, int angle_sec) |
{ |
|
servo_table[servo].max_angle_sec = angle_sec; |
return 0; |
|
} |
|
int servo_set_min_angle(int servo, int angle_sec) |
{ |
|
servo_table[servo].min_angle_sec = angle_sec; |
return 0; |
|
} |
|
/* 0000.0ppp:0000.vvvv:vvvv.vvvv */ |
int servo_set_angle(int servo, int angle) |
int servo_set_angle_sec(int servo, int angle_sec) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
int err, angle_tick; |
|
if (servo > 7) return -1; |
|
if (angle_sec > servo_table[servo].max_angle_sec || |
angle_sec < servo_table[servo].min_angle_sec) return -1; |
|
angle_tick = servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick / |
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
619,12 → 660,12 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | ((angle >> 8) & 0x0F); |
b = 0x00 | ((angle_tick >> 8) & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | (angle & 0xFF); |
b = 0x00 | (angle_tick & 0xFF); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
638,12 → 679,40 |
|
} |
|
/* 0001.0ppp */ |
int servo_store_default_position(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 = 0x10 | (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; |
|
} |
|
/* 0000.1ppp */ |
int servo_get_angle(int servo) |
int servo_get_angle_sec(int servo) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,res; |
int err,res,data; |
|
if (servo > 7) return -1; |
|
661,8 → 730,12 |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
data = (res - servo_table[servo].zero_tick) * |
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) / |
servo_table[servo].delta_tick; |
|
if (!timer_expired) |
return res; |
return data; |
else |
return -1; |
|