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); }