Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 726 → Rev 725

/shark/trunk/ports/servo/servo.c
129,8 → 129,8
{-324000, 324000, 1200, 1600}}};
 
#define RXTX_BUFF_MAX 100
volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
volatile static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
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 = 0;
159,7 → 159,6
if (RXTX_addr[port][RX_position[port]] == NULL) {
if (data != RXTX_buff[port][RX_position[port]])
servo_rx_error(port,1);
 
RX_position[port]++;
 
if (RX_position[port] >= RXTX_BUFF_MAX) {
172,8 → 171,6
 
} else {
*RXTX_addr[port][RX_position[port]] = data;
RXTX_buff[port][RX_position[port]] = 1; //Unlock the data
 
RX_position[port]++;
 
if (RX_position[port] >= RXTX_BUFF_MAX) {
183,7 → 180,6
if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) ||
(RX_cycle > TX_cycle))
servo_rx_error(port,2);
 
}
 
}
197,7 → 193,7
 
int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) {
 
int i = 0, old, oldcycle, RX = 0;
int i = 0, old, oldcycle, RX;
SYS_FLAGS f;
 
f = kern_fsave();
224,9 → 220,8
 
i = 0;
while(i < len_res) {
RXTX_buff[port][TX_position[port]] = 0; //Lock the data
RXTX_buff[port][TX_position[port]] = 0;
RXTX_addr[port][TX_position[port]] = res+i;
RX = TX_position[port];
TX_position[port]++;
if (TX_position[port] >= RXTX_BUFF_MAX) {
TX_cycle++;
241,6 → 236,8
i++;
}
 
RX = (int)RX_position;
 
kern_frestore(f);
 
com_irq_send(port, len_msg, msg);
249,10 → 246,18
 
}
 
int servo_wait(unsigned port, int RX) {
int servo_wait(unsigned port, int RX, int number) {
 
int f = RX, i = 0;
 
while (i < number) {
f++;
if (f >= RXTX_BUFF_MAX) f = 0;
i++;
}
 
/* Active wait until number bytes received */
while (RXTX_buff[port][RX] == 0);
while (RX_position[port] < f);
 
return 0;
 
335,7 → 340,7
b[0] = 0x85;
RX = servo_send_msg(servo_port, b, 1, r, 2);
 
servo_wait(servo_port, RX);
servo_wait(servo_port, RX, 2);
 
res_w = r[0]; /* bit W */
res_b = r[1]; /* byte SPBRG */
390,7 → 395,7
b[0] = 0x89;
RX = servo_send_msg(servo_port, b, 1, r, 1);
 
servo_wait(servo_port, RX);
servo_wait(servo_port, RX, 1);
 
res = r[0];
 
421,7 → 426,7
b[0] = 0x8C;
RX = servo_send_msg(servo_port, b, 1, r, 1);
 
servo_wait(servo_port, RX);
servo_wait(servo_port, RX, 1);
 
res = r[0];
 
528,7 → 533,7
b[1] = 0x40 | (0x01 & bank);
RX = servo_send_msg(servo_port, b, 2, r, 1);
servo_wait(servo_port, RX);
servo_wait(servo_port, RX, 1);
 
res = r[0];
 
624,9 → 629,9
b[0] = 0x10 | (servo & 0x0F);
RX = servo_send_msg(servo_port, b, 1, r, 2);
 
servo_wait(servo_port, RX);
servo_wait(servo_port, RX, 2);
 
res = (int)(r[0]) << 8;
res = r[0] << 8;
res |= r[1];
 
data = ((res*TICK_LEN/1000) - servo_table[port][servo].zero_tick) *
649,7 → 654,7
b[0] = 0x40 | (adport & 0x07);
RX = servo_send_msg(servo_port, b, 1, r, 2);
 
servo_wait(servo_port, RX);
servo_wait(servo_port, RX, 2);
 
res = r[0] << 8;
res |= r[1];