Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 357 → Rev 358

/shark/trunk/ports/servo/servo.c
57,9 → 57,9
 
#define barrier() __asm__ __volatile__("" ::: "memory");
 
#define SERVO_DEBUG
//#define SERVO_DEBUG
 
#define SERVO_TIMEOUT 100000 /* us */
#define SERVO_TIMEOUT 200000 /* us */
 
#define SERVO_SPEED 38400
#define SERVO_PARITY NONE
66,6 → 66,8
#define SERVO_LEN 8
#define SERVO_STOP 1
 
#define SERVO_CLOCK 20000000 /* 20MHz */
 
#define TICK_LEN 1600 /* ns */
#define TICK_LEN_PERIOD 51200 /* ns */
 
96,7 → 98,6
 
int timer_expired = 0;
int timeout_event;
int servo_port;
 
const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
 
112,37 → 113,6
115200,
-1};
 
/* 20MHz: SPBRG, BAUD BRGH=0, BAUD BRGH=1 */
const int Baud20MhzData[] = {
255, 1221, 4882,
254, 1225, 4902,
253, 1230, 4921,
252, 1235, 4941,
133, 2332, 9328,
132, 2350, 9398,
131, 2367, 9470,
130, 2385, 9542,
129, 2404, 9615,
128, 2422, 9690,
127, 2441, 9766,
126, 2461, 9842,
88, 0, 14045,
87, 0, 14204,
86, 0, 14368,
85, 0, 14535,
84, 0, 14706,
66, 4664, 18657,
65, 4735, 18939,
64, 4808, 19231,
63, 4883, 19531,
32, 9470, 37879,
31, 9766, 39062,
21, 14205, 56818,
15, 19531, 0,
10, 0, 113636,
7, 39062, 0,
-1, -1, -1};
 
void set_timer_expired(void *arg)
{
timeout_event = NIL;
238,22 → 208,21
 
}
 
int servo_open(int port)
int servo_open(int port,int speed)
{
int err;
 
servo_port = (unsigned)(port);
err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
 
return err;
 
}
 
int servo_close(void)
int servo_close(int port)
{
int err;
 
err = com_close(servo_port);
err = com_close((unsigned)(port));
 
return err;
 
260,12 → 229,13
}
 
/* 1000.011w:bbbb.bbbb */
int servo_set_RS232_baudrate(int baud)
int servo_set_RS232_baudrate(int port, int baud)
{
struct timespec current_time;
unsigned char b;
int err,diff,mindiff,i;
int err, spbrg_temp, i;
unsigned char spbrg, w;
int servo_port = (unsigned)(port);
 
i = 0;
while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
274,35 → 244,24
return -1;
}
 
i = 0;
mindiff = 0xFFFFFF;
spbrg = 0;
w = 0;
while(Baud20MhzData[i] != -1) {
diff = abs(Baud20MhzData[i+1] - baud);
if (diff < mindiff) {
mindiff = diff;
spbrg = (unsigned char)(Baud20MhzData[i]);
w = 0;
}
diff = abs(Baud20MhzData[i+2] - baud);
if (diff < mindiff) {
mindiff = diff;
spbrg = (unsigned char)(Baud20MhzData[i]);
w = 1;
}
i += 3;
w = 1;
spbrg_temp = (SERVO_CLOCK / (16*baud)) - 1;
if (spbrg_temp>255) {
w = 0;
spbrg = (SERVO_CLOCK / (64*baud)) - 1;
} else {
spbrg = spbrg_temp;
}
 
#ifdef SERVO_DEBUG
kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
#endif
#endif
 
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
b = 0x86 | (w & 0x01);
err = com_send(servo_port, b);
err = com_receive(servo_port);
314,10 → 273,10
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
com_close(servo_port);
com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
 
/*com_close(servo_port);
com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);*/
 
if (!timer_expired)
return 0;
else
326,25 → 285,31
}
 
/* 1000.0101 */
int servo_get_RS232_baudrate(void)
int servo_get_RS232_baudrate(int port)
{
struct timespec current_time;
unsigned char b;
int err,res;
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);
 
b = 0x85;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
res = com_receive(servo_port);
res_w = com_receive(servo_port); /* bit W */
res_b = com_receive(servo_port); /* byte SPBRG */
if (res_w)
res = SERVO_CLOCK / ( 16 * (res_b + 1) );
else
res = SERVO_CLOCK / ( 64 * (res_b + 1) );
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return res;
else
353,55 → 318,57
}
 
/* 1000.0100 */
int servo_store_RS232_baudrate(void)
int servo_store_RS232_baudrate(int port)
{
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);
 
b = 0x84;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
return -1;
return -1;
 
}
 
/* 1000.1010:llll.llll */
int servo_set_period(int period)
int servo_set_period(int port, int period)
{
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);
 
b = 0x8A;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = (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;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
410,25 → 377,26
}
 
/* 1000.1001 */
int servo_get_period(void)
int servo_get_period(int port)
{
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);
 
b = 0x89;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
res = com_receive(servo_port);
res = com_receive(servo_port);
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8);
else
437,41 → 405,44
}
 
/* 1000.1000 */
int servo_store_period(void)
int servo_store_period(int port)
{
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);
 
b = 0x88;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
return -1;
return -1;
 
}
 
/* 1000.1100 */
int servo_get_setup_switch(void)
int servo_get_setup_switch(int port)
{
struct timespec current_time;
unsigned char b;
int err,res;
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);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
b = 0x8C;
err = com_send(servo_port, b);
489,12 → 460,13
}
 
/* 1000.111s */
int servo_set_RC5_switch(int data)
int servo_set_RC5_switch(int port, int data)
{
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);
505,8 → 477,8
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
515,13 → 487,15
}
 
/* 1000.0000:0000.Mmmm */
int servo_turn_off(int servo)
int servo_turn_off(int port, int servo)
{
 
struct timespec current_time;
unsigned char b;
int err;
if (servo > 15) return -1;
int err;
int servo_port = (unsigned)(port);
 
if (servo > 15) return -1;
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
538,7 → 512,7
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
547,201 → 521,207
}
 
/* 1000.0000:0001.Mmmm */
int servo_turn_on(int servo)
int servo_turn_on(int port, int servo)
{
 
struct timespec current_time;
unsigned char b;
int err;
int servo_port = (unsigned)(port);
 
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);
 
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x10 | (servo & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
return -1;
 
}
 
/* 1000.0000:0010.0000 */
int servo_turn_off_all(void)
int servo_turn_off_all(int port)
{
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);
 
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x20;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
return -1;
 
}
 
/* 1000.0000:0010.0001 */
int servo_turn_on_all(void)
int servo_turn_on_all(int port)
{
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);
 
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x21;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
return -1;
 
}
 
/* 1000.0000:0101.000M:mmmm.mmmm */
int servo_set_levels(int bank,int mask)
int servo_set_levels(int port, int bank,int mask)
{
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);
 
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x50 | (0x01 & bank);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = (unsigned char)(mask & 0xFF);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
return -1;
 
}
 
/* 1000.0000:0100.000M */
int servo_get_levels(int bank)
int servo_get_levels(int port, int bank)
{
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);
 
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x40 | (0x01 & bank);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
err = com_receive(servo_port);
err = com_receive(servo_port);
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return err;
else
return -1;
 
}
 
/* 1000.0000:1000.0000 */
int servo_store_levels(void)
int servo_store_levels(int port)
{
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);
 
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
return -1;
 
}
 
int servo_set_max_angle(int servo, int angle_sec)
int servo_set_max_angle(int port, int servo, int angle_sec)
{
 
servo_table[servo].max_angle_sec = angle_sec;
758,18 → 738,19
}
 
/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
int servo_set_angle_sec(int servo, int angle_sec)
int servo_set_angle_sec(int port, int servo, int angle_sec)
{
struct timespec current_time;
unsigned char b;
int err, angle_tick;
int servo_port = (unsigned)(port);
 
if (servo > 15) return -1;
 
if (angle_sec > servo_table[servo].max_angle_sec ||
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 /
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;
776,7 → 757,7
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);
err = com_receive(servo_port);
790,7 → 771,7
b = 0x00 | (angle_tick & 0xFF);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
802,26 → 783,27
}
 
/* 0010.Pppp */
int servo_store_default_position(int servo)
int servo_store_default_position(int port, int servo)
{
struct timespec current_time;
unsigned char b;
int err;
int servo_port = (unsigned)(port);
 
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);
 
b = 0x20 | (servo & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return 0;
else
830,14 → 812,15
}
 
/* 0001.Pppp */
int servo_get_angle_sec(int servo)
int servo_get_angle_sec(int port, int servo)
{
struct timespec current_time;
unsigned char b;
int err,res,data;
int servo_port = (unsigned)(port);
 
if (servo > 15) return -1;
 
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
852,7 → 835,7
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
data = ((res*TICK_LEN/1000) - 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;
 
864,21 → 847,22
}
 
/* 0100:0aaa */
int servo_get_analog(int port)
int servo_get_analog(int port, int adport)
{
 
struct timespec current_time;
unsigned char b;
int err,res;
int servo_port = (unsigned)(port);
 
if (port > 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);
 
b = 0x40 | (port & 0x07);
b = 0x40 | (adport & 0x07);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
/shark/trunk/ports/servo/include/servo.h
1,26 → 1,28
#ifndef __SERVO_H__
#define __SERVO_H__
 
#define SERVO_COM1 0
#define SERVO_COM2 1
#define SERVO_COM3 2
#define SERVO_COM4 3
#ifndef COM1
#define COM1 0
#define COM2 1
#define COM3 2
#define COM4 3
#endif
 
int servo_open(int port);
int servo_close(void);
int servo_open(int port, int speed);
int servo_close(int port);
 
/* Setup */
 
int servo_set_RS232_baudrate(int baud); /* BaudRate */
int servo_get_RS232_baudrate(void);
int servo_store_RS232_baudrate(void);
int servo_set_RS232_baudrate(int port, int baud); /* BaudRate */
int servo_get_RS232_baudrate(int port);
int servo_store_RS232_baudrate(int port);
 
int servo_set_period(int period); /* Servo period in us */
int servo_get_period(void);
int servo_store_period(void);
int servo_set_period(int port, int period); /* Servo period in us */
int servo_get_period(int port);
int servo_store_period(int port);
 
int servo_get_setup_switch(void); /* RC0 RC1 RC2 */
int servo_set_RC5_switch(int data);
int servo_get_setup_switch(int port); /* RC0 RC1 RC2 */
int servo_set_RC5_switch(int port, int data);
 
/* Servo control */
 
27,28 → 29,28
/* Convert angle (degree, minute, second -> second) */
#define ANGLE2SEC(deg,min,sec) ((deg)*3600 + (min)*60 + (sec))
 
int servo_turn_off(int servo);
int servo_turn_on(int servo);
int servo_turn_off_all(void);
int servo_turn_on_all(void);
int servo_turn_off(int port, int servo);
int servo_turn_on(int port, int servo);
int servo_turn_off_all(int port);
int servo_turn_on_all(int port);
 
int servo_set_levels(int bank, int mask);
int servo_get_levels();
int servo_store_levels();
int servo_set_levels(int port, int bank, int mask);
int servo_get_levels(int port, int bank);
int servo_store_levels(int port);
 
int servo_set_max_angle_sec(int servo, int angle_sec);
int servo_set_min_angle_sec(int servo, int angle_sec);
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_angle_sec(int servo, int angle_sec);
int servo_get_angle_sec(int servo);
int servo_set_angle_sec(int port, int servo, int angle_sec);
int servo_get_angle_sec(int port, int servo);
 
int servo_store_default_position(int servo);
int servo_store_default_position(int port, int servo);
 
/* Analog control */
 
#define MAX_ANALOG 0x03FF
 
int servo_get_analog(int port);
int servo_get_analog(int port, int adport);
 
#endif