int get_voltage() {
int result, value;
result = one_sensor_read("V", &value);
if (result < 0)
return result;
return value;
}
int check_bump(int side) {
int result = 0, value;
switch(side) {
case LEFT:
result = one_sensor_read("BFL", &value);
break;
case RIGHT:
result = one_sensor_read("BFR", &value);
break;
default:
fprintf(stderr, "get_front_ir_dist: invalid side number %d\n", side);
exit(1);
}
if (result < 0)
return result;
return value;
}
int get_us_dist() {
int result, value;
result = one_sensor_read("US", &value);
if (result < 0)
/* something broke - return 5 meters */
return 500;
return value;
}
int get_side_ir_dist(int side) {
int result = 0, value;
switch(side) {
case LEFT:
result = one_sensor_read("ISL", &value);
break;
case RIGHT:
result = one_sensor_read("ISR", &value);
break;
default:
fprintf(stderr, "get_front_ir_dist: invalid side number %d\n", side);
exit(1);
}
if (result < 0)
/* something broke - return 50cm */
return 50;
return gp2d120_to_dist(value);
}
int one_sensor_read(char *sensorname, int *value) {
char sendbuf[READBUFLEN];
char recvbuf[READBUFLEN];
const char *arg;
const char *reply_msg;
sprintf(sendbuf, "S %s\n", sensorname);
if (send_msg(sendbuf, strlen(sendbuf))) {
/* now we loop, reading until we get the answer to the request we just asked. */
memset(recvbuf, 0, READBUFLEN);
while (1) {
reply_msg = recv_msg(recvbuf, READBUFLEN);
/* compare request string to response */
if (strncmp(reply_msg, sendbuf, strlen(sendbuf)-1)==0) {
/* skip over the fixed part of the response to get to the
returned value */
arg = reply_msg + strlen(sendbuf);
*value = atoi(arg);
/* all done */
check_errors();
return 0;
} else if (reply_msg[0] == 'W') {
/* response is an asynchronous warning - we'll print it and ignore it. */
printf("Warning: %s", reply_msg+2);
/* now go round the loop again and re-read */
} else {
/* what we got back wasn't the response we expected or a warning. */
printf("got an error >>%s<<\n", reply_msg);
/* we don't really know what happened - just clear any
remaining input in the hope we can get back in sync. */
clear_input_stream();
return -1;
}
}
} else {
/* the send failed - retry the whole request; it should auto-reconnect */
printf("send failed!!!\n");
return one_sensor_read(sensorname, value);
}
}
void inputMotors(){
one_sensor_read("MEL", &currL);
one_sensor_read("MER", &currR);
}
