36,7 → 36,6 |
* |
*/ |
|
#include <ll/sys/ll/ll-instr.h> |
#include "kernel/kern.h" |
|
#include "drivers/scom.h" |
45,10 → 44,7 |
#include "servo.h" |
|
//#define SERVO_DEBUG |
//#define SERVO_TIMEOUT_EVENT |
|
#define SERVO_TIMEOUT 200000 /* us */ |
|
#define SERVO_SPEED 19200 |
#define SERVO_PARITY NONE |
#define SERVO_LEN 8 |
132,9 → 128,6 |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}}}; |
|
int timer_expired = 0; |
int timeout_event; |
|
#define RXTX_BUFF_MAX 100 |
static BYTE RXTX_buff[4][RXTX_BUFF_MAX]; |
static BYTE *RXTX_addr[4][RXTX_BUFF_MAX]; |
155,12 → 148,6 |
115200, |
-1}; |
|
void set_timer_expired(void *arg) |
{ |
timeout_event = NIL; |
timer_expired = 1; |
} |
|
void servo_rx_error(unsigned port, unsigned type) { |
|
kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type); |
277,6 → 264,8 |
{ |
int err; |
|
com_irq_disable((unsigned)(port),ALL_IRQ); |
|
com_close_irq((unsigned)(port)); |
|
err = com_close((unsigned)(port)); |
288,11 → 277,8 |
/* 1000.011w:bbbb.bbbb */ |
int servo_set_RS232_baudrate(int port, int baud) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err, spbrg_temp, i; |
unsigned char b[2]; |
int spbrg_temp, i; |
unsigned char spbrg, w; |
int servo_port = (unsigned)(port); |
|
316,66 → 302,27 |
kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w); |
#endif |
|
timer_expired = 0; |
b[0] = 0x86 | (w & 0x01); |
b[1] = spbrg; |
servo_send_msg(servo_port, b, 2, NULL, 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 |
return 0; |
|
b = 0x86 | (w & 0x01); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = spbrg; |
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); |
|
/*com_close(servo_port); |
com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);*/ |
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
} |
|
/* 1000.0101 */ |
int servo_get_RS232_baudrate(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err, res, res_w, res_b; |
unsigned char b[1],r[2]; |
int 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 |
b[0] = 0x85; |
servo_send_msg(servo_port, b, 1, r, 2); |
|
b = 0x85; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
res_w = r[0]; /* bit W */ |
res_b = r[1]; /* byte SPBRG */ |
|
res_w = com_receive(servo_port); /* bit W */ |
res_b = com_receive(servo_port); /* byte SPBRG */ |
|
if (res_w != -1 && res_b != -1) { |
if (res_w) |
res = SERVO_CLOCK / ( 16 * (res_b + 1) ); |
385,270 → 332,114 |
return -1; |
} |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
return res; |
|
if (!timer_expired) |
return res; |
else |
return -1; |
|
} |
|
/* 1000.0100 */ |
int servo_store_RS232_baudrate(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[1]; |
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 |
|
b[0] = 0x84; |
servo_send_msg(servo_port, b, 1, NULL, 0); |
|
return 0; |
|
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; |
|
} |
|
/* 1000.1010:llll.llll */ |
int servo_set_period(int port, int period) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[2]; |
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 |
b[0] = 0x8A; |
b[1] = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF; |
servo_send_msg(servo_port, b, 2, NULL, 0); |
|
b = 0x8A; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
return 0; |
|
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 |
return -1; |
|
} |
|
/* 1000.1001 */ |
int servo_get_period(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err,res; |
unsigned char b[1],r[1]; |
int res; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
b[0] = 0x89; |
servo_send_msg(servo_port, b, 1, r, 1); |
|
#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 |
res = r[0]; |
|
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); |
return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
else |
return -1; |
|
} |
|
/* 1000.1000 */ |
int servo_store_period(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[1]; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
b[0] = 0x88; |
servo_send_msg(servo_port, b, 1, NULL, 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 |
return 0; |
|
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; |
|
} |
|
/* 1000.1100 */ |
int servo_get_setup_switch(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err,res; |
unsigned char b[1],r[1]; |
int 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 |
b[0] = 0x8C; |
servo_send_msg(servo_port, b, 1, r, 1); |
|
b = 0x8C; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(servo_port); |
res = r[0]; |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
return res; |
|
if (!timer_expired) |
return res; |
else |
return -1; |
|
} |
|
/* 1000.111s */ |
int servo_set_RC5_switch(int port, int data) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[1]; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
b[0] = 0x8E | (data & 0x01); |
servo_send_msg(servo_port, b, 1, NULL, 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 |
return 0; |
|
b = 0x8E | (data & 0x01); |
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:0000.Mmmm */ |
int servo_turn_off(int port, int servo) |
{ |
|
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[2]; |
int servo_port = (unsigned)(port); |
|
if (servo > 15) return -1; |
timer_expired = 0; |
|
b[0] = 0x80; |
b[1] = 0x00 | (servo & 0x0F); |
servo_send_msg(servo_port, b, 2, NULL, 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 |
return 0; |
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | (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:0001.Mmmm */ |
int servo_turn_on(int port, int servo) |
{ |
|
unsigned char b[2]; |
int servo_port = (unsigned)(port); |
|
665,202 → 456,75 |
/* 1000.0000:0010.0000 */ |
int servo_turn_off_all(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[2]; |
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 |
b[0] = 0x80; |
b[1] = 0x20; |
servo_send_msg(servo_port, b, 2, NULL, 0); |
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
return 0; |
|
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(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[2]; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
b[0] = 0x80; |
b[1] = 0x21; |
servo_send_msg(servo_port, b, 2, NULL, 0); |
|
return 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 |
|
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 port, int bank,int mask) |
int servo_set_levels(int port, int bank, int mask) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[3]; |
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 |
b[0] = 0x80; |
b[1] = 0x50 | (0x01 & bank); |
b[2] = (unsigned char)(mask & 0xFF); |
servo_send_msg(servo_port, b, 3, NULL, 0); |
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
return 0; |
|
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 port, int bank) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[2],r[1]; |
int res; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
b[0] = 0x80; |
b[1] = 0x40 | (0x01 & bank); |
servo_send_msg(servo_port, b, 2, r, 1); |
|
#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 |
res = r[0]; |
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
return res; |
|
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); |
|
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(int port) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[2]; |
int servo_port = (unsigned)(port); |
|
timer_expired = 0; |
b[0] = 0x80; |
b[1] = 0x80; |
servo_send_msg(servo_port, b, 2, NULL, 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 |
return 0; |
|
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_sec(int port, int servo, int angle_sec) |
913,78 → 577,38 |
/* 0010.Pppp */ |
int servo_store_default_position(int port, int servo) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[1]; |
int servo_port = (unsigned)(port); |
|
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 |
|
b = 0x20 | (servo & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
b[0] = 0x20 | (servo & 0x0F); |
servo_send_msg(servo_port, b, 1, NULL, 0); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
return 0; |
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
} |
|
/* 0001.Pppp */ |
int servo_get_angle_sec(int port, int servo) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err,res,data; |
unsigned char b[1],r[2]; |
int res,data; |
int servo_port = (unsigned)(port); |
|
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 |
b[0] = 0x10 | (servo & 0x0F); |
servo_send_msg(servo_port, b, 1, r, 2); |
|
b = 0x10 | (servo & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(servo_port) << 8; |
res |= com_receive(servo_port); |
res = r[0] << 8; |
res |= r[1]; |
|
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; |
|
if (!timer_expired) |
return data; |
else |
return -1; |
return data; |
|
} |
|
991,39 → 615,19 |
/* 0100:0aaa */ |
int servo_get_analog(int port, int adport) |
{ |
|
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err,res; |
unsigned char b[1],r[2]; |
int res; |
int servo_port = (unsigned)(port); |
|
if (adport > 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 |
b[0] = 0x40 | (adport & 0x07); |
servo_send_msg(servo_port, b, 1, r, 2); |
|
b = 0x40 | (adport & 0x07); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(servo_port) << 8; |
res |= com_receive(servo_port); |
res = r[0] << 8; |
res |= r[1]; |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
return res; |
|
if (!timer_expired) |
return res; |
else |
return -1; |
|
} |
|