67,6 → 67,7 |
#define SERVO_STOP 1 |
|
#define TICK_LEN 1600 /* ns */ |
#define TICK_LEN_PERIOD 51200 /* ns */ |
|
struct servo_data { |
int min_angle_sec; |
76,14 → 77,14 |
}; |
|
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}}; |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}}; |
|
int timer_expired = 0; |
int timeout_event; |
386,7 → 387,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = (period*1000)/TICK_LEN/8 & 0xFF; |
b = 0xFF - ((period*1000)/TICK_LEN_PERIOD/8 & 0xFF); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
421,7 → 422,7 |
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return (res*TICK_LEN/1000*8); |
return ((0xFF - (unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
else |
return -1; |
|
696,7 → 697,7 |
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return 0; |
return err; |
else |
return -1; |
|
761,8 → 762,8 |
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); |
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)) * 1000 / TICK_LEN; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
844,7 → 845,7 |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
data = (res - servo_table[servo].zero_tick) * |
data = ((res*TICK_LEN/1000) - servo_table[servo].zero_tick) * |
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) / |
servo_table[servo].delta_tick; |
|