Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 380 → Rev 381

/shark/trunk/ports/servo/servo.c
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(&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);
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(&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 = 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(&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 = 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(&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 = 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(&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);
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(&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);
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(&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 = 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(&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);
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(&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);
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(&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);
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(&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);
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(&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);
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(&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);
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(&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);
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(&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);
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(&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 = 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(&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 = 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(&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 = 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(&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 = 0x40 | (adport & 0x07);
err = com_send(servo_port, b);
/shark/trunk/ports/servo/include/servo.h
40,6 → 40,7
 
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);