7,7 → 7,6 |
* |
* Authors : |
* Giacomo Guidi <giacomo@gandalf.sssup.it> |
* Mauro Marinoni <mauro.marinoni@unipv.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
59,7 → 58,6 |
#define barrier() __asm__ __volatile__("" ::: "memory"); |
|
//#define SERVO_DEBUG |
#define SERVO_TIMEOUT_EVENT |
|
#define SERVO_TIMEOUT 200000 /* us */ |
|
80,8 → 78,7 |
int zero_tick; |
}; |
|
struct servo_data servo_table[4][16] = { |
{{-324000, 324000, 1200, 1600}, |
struct servo_data servo_table[] = { |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
96,55 → 93,8 |
{-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}, |
{-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}, |
{-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}, |
{-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}, |
{-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}, |
{-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}, |
{-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; |
281,9 → 231,7 |
/* 1000.011w:bbbb.bbbb */ |
int servo_set_RS232_baudrate(int port, int baud) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err, spbrg_temp, i; |
unsigned char spbrg, w; |
310,15 → 258,10 |
#endif |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
|
b = 0x86 | (w & 0x01); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
344,22 → 287,15 |
/* 1000.0101 */ |
int servo_get_RS232_baudrate(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err, res, res_w, res_b; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
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); |
384,22 → 320,15 |
/* 1000.0100 */ |
int servo_store_RS232_baudrate(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
|
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); |
418,22 → 347,16 |
/* 1000.1010:llll.llll */ |
int servo_set_period(int port, int period) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
|
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); |
456,23 → 379,16 |
/* 1000.1001 */ |
int servo_get_period(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err,res; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
|
b = 0x89; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
491,23 → 407,16 |
/* 1000.1000 */ |
int servo_store_period(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
|
b = 0x88; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
525,22 → 434,15 |
/* 1000.1100 */ |
int servo_get_setup_switch(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err,res; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x8C; |
err = com_send(servo_port, b); |
560,23 → 462,16 |
/* 1000.111s */ |
int servo_set_RC5_switch(int port, int data) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
|
b = 0x8E | (data & 0x01); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
595,9 → 490,7 |
int servo_turn_off(int port, int servo) |
{ |
|
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
604,14 → 497,9 |
|
if (servo > 15) return -1; |
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
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); |
636,9 → 524,7 |
int servo_turn_on(int port, int servo) |
{ |
|
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
646,14 → 532,9 |
if (servo > 15) return -1; |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
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); |
677,22 → 558,15 |
/* 1000.0000:0010.0000 */ |
int servo_turn_off_all(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
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); |
716,23 → 590,16 |
/* 1000.0000:0010.0001 */ |
int servo_turn_on_all(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
755,22 → 622,15 |
/* 1000.0000:0101.000M:mmmm.mmmm */ |
int servo_set_levels(int port, int bank,int mask) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
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); |
799,22 → 659,15 |
/* 1000.0000:0100.000M */ |
int servo_get_levels(int port, int bank) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
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); |
839,22 → 692,15 |
/* 1000.0000:1000.0000 */ |
int servo_store_levels(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
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); |
878,34 → 724,23 |
int servo_set_max_angle(int port, int servo, int angle_sec) |
{ |
|
servo_table[port][servo].max_angle_sec = angle_sec; |
servo_table[servo].max_angle_sec = angle_sec; |
return 0; |
|
} |
|
int servo_set_min_angle(int port, int servo, int angle_sec) |
int servo_set_min_angle(int servo, int angle_sec) |
{ |
|
servo_table[port][servo].min_angle_sec = angle_sec; |
servo_table[servo].min_angle_sec = angle_sec; |
return 0; |
|
} |
|
int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick) |
{ |
|
if (zero_tick != -1) servo_table[port][servo].zero_tick = zero_tick; |
if (delta_tick != -1) servo_table[port][servo].delta_tick = delta_tick; |
return 0; |
|
} |
|
/* 0000.Pppp:0000.vvvv:vvvv.vvvv */ |
int servo_set_angle_sec(int port, int servo, int angle_sec) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err, angle_tick; |
int servo_port = (unsigned)(port); |
912,23 → 747,16 |
|
if (servo > 15) return -1; |
|
if (angle_sec > servo_table[port][servo].max_angle_sec || |
angle_sec < servo_table[port][servo].min_angle_sec) 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[port][servo].zero_tick + angle_sec * |
servo_table[port][servo].delta_tick / |
(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) * |
1000 / TICK_LEN; |
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; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x00 | (servo & 0x0F); |
err = com_send(servo_port, b); |
957,9 → 785,7 |
/* 0010.Pppp */ |
int servo_store_default_position(int port, int servo) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
967,15 → 793,10 |
if (servo > 15) return -1; |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
|
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x20 | (servo & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
993,9 → 814,7 |
/* 0001.Pppp */ |
int servo_get_angle_sec(int port, int servo) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err,res,data; |
int servo_port = (unsigned)(port); |
1003,14 → 822,9 |
if (servo > 15) return -1; |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x10 | (servo & 0x0F); |
err = com_send(servo_port, b); |
1021,9 → 835,9 |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
data = ((res*TICK_LEN/1000) - servo_table[port][servo].zero_tick) * |
(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) / |
servo_table[port][servo].delta_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; |
|
if (!timer_expired) |
return data; |
1036,24 → 850,17 |
int servo_get_analog(int port, int adport) |
{ |
|
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
struct timespec current_time; |
unsigned char b; |
int err,res; |
int servo_port = (unsigned)(port); |
|
if (adport > 7) return -1; |
if (port > 7) return -1; |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x40 | (adport & 0x07); |
err = com_send(servo_port, b); |