Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 381 → Rev 380

/shark/trunk/ports/servo/servo.c
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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
#ifdef SERVO_TIMEOUT_EVENT
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
#ifdef SERVO_TIMEOUT_EVENT
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
#ifdef SERVO_TIMEOUT_EVENT
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
#ifdef SERVO_TIMEOUT_EVENT
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
#ifdef SERVO_TIMEOUT_EVENT
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
#else
timeout_event = NIL;
#endif
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
b = 0x40 | (adport & 0x07);
err = com_send(servo_port, b);
/shark/trunk/ports/servo/include/servo.h
40,7 → 40,6
 
int servo_set_max_angle_sec(int port, int servo, int angle_sec);
int servo_set_min_angle_sec(int port, int servo, int angle_sec);
int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick);
 
int servo_set_angle_sec(int port, int servo, int angle_sec);
int servo_get_angle_sec(int port, int servo);