7,6 → 7,7 |
* |
* 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) |
58,6 → 59,7 |
#define barrier() __asm__ __volatile__("" ::: "memory"); |
|
//#define SERVO_DEBUG |
#define SERVO_TIMEOUT_EVENT |
|
#define SERVO_TIMEOUT 200000 /* us */ |
|
78,7 → 80,8 |
int zero_tick; |
}; |
|
struct servo_data servo_table[] = { |
struct servo_data servo_table[4][16] = { |
{{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}, |
93,8 → 96,55 |
{-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; |
231,7 → 281,9 |
/* 1000.011w:bbbb.bbbb */ |
int servo_set_RS232_baudrate(int port, int baud) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err, spbrg_temp, i; |
unsigned char spbrg, w; |
258,10 → 310,15 |
#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); |
287,15 → 344,22 |
/* 1000.0101 */ |
int servo_get_RS232_baudrate(int port) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err, res, res_w, res_b; |
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 = 0x85; |
err = com_send(servo_port, b); |
320,15 → 384,22 |
/* 1000.0100 */ |
int servo_store_RS232_baudrate(int port) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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 = 0x84; |
err = com_send(servo_port, b); |
347,16 → 418,22 |
/* 1000.1010:llll.llll */ |
int servo_set_period(int port, int period) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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 = 0x8A; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
379,16 → 456,23 |
/* 1000.1001 */ |
int servo_get_period(int port) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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); |
407,16 → 491,23 |
/* 1000.1000 */ |
int servo_store_period(int port) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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); |
434,15 → 525,22 |
/* 1000.1100 */ |
int servo_get_setup_switch(int port) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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 = 0x8C; |
err = com_send(servo_port, b); |
462,16 → 560,23 |
/* 1000.111s */ |
int servo_set_RC5_switch(int port, int data) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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); |
490,7 → 595,9 |
int servo_turn_off(int port, int servo) |
{ |
|
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
497,9 → 604,14 |
|
if (servo > 15) 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); |
|
#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); |
524,7 → 636,9 |
int servo_turn_on(int port, int servo) |
{ |
|
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
532,9 → 646,14 |
if (servo > 15) 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); |
|
#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); |
558,15 → 677,22 |
/* 1000.0000:0010.0000 */ |
int servo_turn_off_all(int port) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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); |
590,16 → 716,23 |
/* 1000.0000:0010.0001 */ |
int servo_turn_on_all(int port) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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); |
622,15 → 755,22 |
/* 1000.0000:0101.000M:mmmm.mmmm */ |
int servo_set_levels(int port, int bank,int mask) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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); |
659,15 → 799,22 |
/* 1000.0000:0100.000M */ |
int servo_get_levels(int port, int bank) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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); |
692,15 → 839,22 |
/* 1000.0000:1000.0000 */ |
int servo_store_levels(int port) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
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); |
724,23 → 878,34 |
int servo_set_max_angle(int port, int servo, int angle_sec) |
{ |
|
servo_table[servo].max_angle_sec = angle_sec; |
servo_table[port][servo].max_angle_sec = angle_sec; |
return 0; |
|
} |
|
int servo_set_min_angle(int servo, int angle_sec) |
int servo_set_min_angle(int port, int servo, int angle_sec) |
{ |
|
servo_table[servo].min_angle_sec = angle_sec; |
servo_table[port][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) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err, angle_tick; |
int servo_port = (unsigned)(port); |
747,16 → 912,23 |
|
if (servo > 15) return -1; |
|
if (angle_sec > servo_table[servo].max_angle_sec || |
angle_sec < servo_table[servo].min_angle_sec) return -1; |
if (angle_sec > servo_table[port][servo].max_angle_sec || |
angle_sec < servo_table[port][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)) * 1000 / TICK_LEN; |
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; |
|
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 = 0x00 | (servo & 0x0F); |
err = com_send(servo_port, b); |
785,7 → 957,9 |
/* 0010.Pppp */ |
int servo_store_default_position(int port, int servo) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
int servo_port = (unsigned)(port); |
793,10 → 967,15 |
if (servo > 15) 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); |
|
|
#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 = 0x20 | (servo & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
814,7 → 993,9 |
/* 0001.Pppp */ |
int servo_get_angle_sec(int port, int servo) |
{ |
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err,res,data; |
int servo_port = (unsigned)(port); |
822,9 → 1003,14 |
if (servo > 15) 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); |
|
#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 = 0x10 | (servo & 0x0F); |
err = com_send(servo_port, b); |
835,9 → 1021,9 |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
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; |
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; |
|
if (!timer_expired) |
return data; |
850,17 → 1036,24 |
int servo_get_analog(int port, int adport) |
{ |
|
struct timespec current_time; |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err,res; |
int servo_port = (unsigned)(port); |
|
if (port > 7) return -1; |
if (adport > 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); |
|
#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 = 0x40 | (adport & 0x07); |
err = com_send(servo_port, b); |