Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 731 → Rev 732

/shark/trunk/ports/servo/servo.c
377,22 → 377,30
b[0] = 0x85;
RX = servo_send_msg(servo_port, b, 1, r, 2);
 
servo_wait(servo_port, RX);
if (RX != -1) {
servo_wait(servo_port, RX);
if (servo_skip[port] == 0) {
 
res_w = r[0]; /* bit W */
res_b = r[1]; /* byte SPBRG */
res_w = r[0]; /* bit W */
res_b = r[1]; /* byte SPBRG */
if (res_w != -1 && res_b != -1) {
if (res_w)
res = SERVO_CLOCK / ( 16 * (res_b + 1) );
else
res = SERVO_CLOCK / ( 64 * (res_b + 1) );
} else {
return -1;
}
if (res_w != -1 && res_b != -1) {
if (res_w)
res = SERVO_CLOCK / ( 16 * (res_b + 1) );
else
res = SERVO_CLOCK / ( 64 * (res_b + 1) );
} else {
return -1;
}
 
return res;
return res;
 
} else return -1;
 
} else return -1;
 
}
 
/* 1000.0100 */
432,12 → 440,20
b[0] = 0x89;
RX = servo_send_msg(servo_port, b, 1, r, 1);
 
servo_wait(servo_port, RX);
if (RX != -1) {
servo_wait(servo_port, RX);
if (servo_skip[port] == 0) {
 
res = r[0];
res = r[0];
 
return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8);
return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8);
 
} else return -1;
 
} else return -1;
 
}
 
/* 1000.1000 */
463,12 → 479,20
b[0] = 0x8C;
RX = servo_send_msg(servo_port, b, 1, r, 1);
 
servo_wait(servo_port, RX);
if (RX != -1) {
servo_wait(servo_port, RX);
if (servo_skip[port] == 0) {
 
res = r[0];
res = r[0];
 
return res;
return res;
 
} else return -1;
 
} else return -1;
 
}
 
/* 1000.111s */
569,13 → 593,21
b[0] = 0x80;
b[1] = 0x40 | (0x01 & bank);
RX = servo_send_msg(servo_port, b, 2, r, 1);
servo_wait(servo_port, RX);
 
res = r[0];
if (RX != -1) {
servo_wait(servo_port, RX);
if (servo_skip[port] == 0) {
 
return res;
res = r[0];
 
return res;
 
} else return -1;
 
} else return -1;
 
}
 
/* 1000.0000:1000.0000 */
700,12 → 732,20
b[0] = 0x40 | (adport & 0x07);
RX = servo_send_msg(servo_port, b, 1, r, 2);
 
servo_wait(servo_port, RX);
if (RX != -1) {
 
res = r[0] << 8;
res |= r[1];
servo_wait(servo_port, RX);
 
return res;
if (servo_skip[port] == 0) {
 
res = (int)r[0] << 8;
res |= r[1];
 
return res;
 
} else return -1;
} else return -1;
 
}