Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 730 → Rev 728

/shark/trunk/ports/servo/servo.c
128,18 → 128,15
{-324000, 324000, 1200, 1600},
{-324000, 324000, 1200, 1600}}};
 
//#define SERVO_TIMEOUT 10000
 
#define RXTX_BUFF_MAX 100
volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
volatile static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
volatile unsigned int RX_position[4] = {0,0,0,0};
volatile unsigned int TX_position[4] = {0,0,0,0};
volatile unsigned int RX_cycle[4] = {0,0,0,0};
volatile unsigned int TX_cycle[4] = {0,0,0,0};
volatile unsigned int RX_cycle = 0;
volatile unsigned int TX_cycle = 0;
volatile unsigned int servo_skip[4] = {0,0,0,0};
volatile unsigned int lock_write[4] = {0,0,0,0};
volatile int servo_timer[4] = {NIL,NIL,NIL,NIL};
 
const int BaudTable[] = {
1200,
168,11 → 165,11
RX_position[port]++;
 
if (RX_position[port] >= RXTX_BUFF_MAX) {
RX_cycle[port]++;
RX_cycle++;
RX_position[port] = 0;
}
if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) ||
(RX_cycle[port] > TX_cycle[port]))
if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) ||
(RX_cycle > TX_cycle))
servo_rx_error(port,2);
 
} else {
182,11 → 179,11
RX_position[port]++;
 
if (RX_position[port] >= RXTX_BUFF_MAX) {
RX_cycle[port]++;
RX_cycle++;
RX_position[port] = 0;
}
if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) ||
(RX_cycle[port] > TX_cycle[port]))
if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) ||
(RX_cycle > TX_cycle))
servo_rx_error(port,3);
 
}
193,15 → 190,18
 
}
 
extern unsigned SCom_Error[4];
extern int SCom_Error;
 
void servo_confirm(unsigned port, BYTE msg_status) {
 
if (msg_status == COM_ERROR) {
kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error[port]);
kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error);
servo_skip[port] = 1;
}
 
lock_write[port] = 0;
if (msg_status == COM_OK) {
lock_write[port] = 0;
}
 
}
 
216,7 → 216,7
f = kern_fsave();
 
old = TX_position[port];
oldcycle = TX_cycle[port];
oldcycle = TX_cycle;
 
while(i < len_msg) {
RXTX_buff[port][TX_position[port]] = msg[i];
223,13 → 223,12
RXTX_addr[port][TX_position[port]] = NULL;
TX_position[port]++;
if (TX_position[port] >= RXTX_BUFF_MAX) {
TX_cycle[port]++;
TX_cycle++;
TX_position[port] = 0;
}
if (TX_position[port] == RX_position[port]) {
TX_position[port] = old;
TX_cycle[port] = oldcycle;
lock_write[port] = 0;
TX_cycle = oldcycle;
kern_frestore(f);
return -1;
}
243,13 → 242,12
RX = TX_position[port];
TX_position[port]++;
if (TX_position[port] >= RXTX_BUFF_MAX) {
TX_cycle[port]++;
TX_cycle++;
TX_position[port] = 0;
}
if (TX_position[port] == RX_position[port]) {
TX_position[port] = old;
TX_cycle[port] = oldcycle;
lock_write[port] = 0;
TX_cycle = oldcycle;
kern_frestore(f);
return -1;
}
264,38 → 262,13
 
}
 
void servo_timeout(void *arg) {
 
unsigned port = (unsigned)(arg);
 
servo_timer[port] = NIL;
servo_skip[port] = 1;
 
kern_printf("(SERVO: TIMEOUT PORT:%d)",port);
}
 
int servo_wait(unsigned port, int RX) {
 
#ifdef SERVO_TIMEOUT
struct timespec acttime;
#endif
servo_skip[port] = 0;
 
#ifdef SERVO_TIMEOUT
kern_gettime(&acttime);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&acttime);
servo_timer[port] = kern_event_post(&acttime,servo_timeout,(void *)(port));
#endif
 
/* Active wait until number bytes received */
while (RXTX_buff[port][RX] == 0 && servo_skip[port] == 0);
 
#ifdef SERVO_TIMEOUT
if (servo_timer[port] != NIL)
kern_event_delete(servo_timer[port]);
#endif
 
return 0;
 
}
621,7 → 594,7
int servo_set_angle_sec(int port, int servo, int angle_sec)
{
unsigned char b[3];
int angle_tick, res = 0;
int angle_tick;
int servo_port = (unsigned)(port);
 
if (servo > 15) return -1;
633,9 → 606,9
b[0] = 0x00 | (servo & 0x0F);
b[1] = 0x00 | ((angle_tick >> 8) & 0x0F);
b[2] = 0x00 | (angle_tick & 0xFF);
res = servo_send_msg(servo_port, b, 3, NULL, 0);
servo_send_msg(servo_port, b, 3, NULL, 0);
 
return res;
return 0;
 
}
 
643,15 → 616,14
int servo_store_default_position(int port, int servo)
{
unsigned char b[1];
int res = 0;
int servo_port = (unsigned)(port);
 
if (servo > 15) return -1;
 
b[0] = 0x20 | (servo & 0x0F);
res = servo_send_msg(servo_port, b, 1, NULL, 0);
servo_send_msg(servo_port, b, 1, NULL, 0);
 
return res;
return 0;
 
}
 
667,25 → 639,17
b[0] = 0x10 | (servo & 0x0F);
RX = servo_send_msg(servo_port, b, 1, r, 2);
 
if (RX != -1) {
servo_wait(servo_port, RX);
 
servo_wait(servo_port, RX);
res = (int)(r[0]) << 8;
res |= r[1];
 
if (servo_skip[port] == 0) {
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;
 
res = (int)(r[0]) << 8;
res |= r[1];
return data;
 
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;
 
return data;
 
} else return -1;
 
} else return -1;
 
}
 
/* 0100:0aaa */