private void scan(String device) throws IOException {
switch (device.toUpperCase()) {
case "SONAR":
device = "S";
break;
case "LASER":
device = "L";
break;
default:
throw new IllegalArgumentException("Invalid device " + device);
}
mobileRobot.sendCommand(device + "1.SCAN");
try {
result = input.readLine();
} catch (InterruptedIOException e) {
result = input.readLine();
}
parseMeasure(result, measures);
if (device.equals("S")) {
occupancyMap.drawSonarScan(position, measures);
} else if (device.equals("L")) {
occupancyMap.drawLaserScan(position, measures);
}
}
private void rotate(String direction) throws IOException {
if (!direction.equals("RIGHT") && !direction.equals("LEFT")) {
throw new IllegalArgumentException("Invalid Direction " + direction);
}
mobileRobot.sendCommand("P1.ROTATE" + direction + " 90");
result = input.readLine();
}
@Override
public void run() {
while (running) {
try {
processScan();
if (scannedMap()) {
mobileRobot.quit();
}
} catch (IOException e) {
e.printStackTrace();
running = false;
}
}
}
public MobileRobotAIV2(MobileRobot mobileRobot, OccupancyMap occupancyMap) {
this.mobileRobot = mobileRobot;
this.occupancyMap = occupancyMap;
try {
pipedInput = new PipedInputStream();
input = new BufferedReader(new InputStreamReader(pipedInput));
output = new PrintWriter(new PipedOutputStream(pipedInput), true);
result = "";
} catch (IOException e) {
e.printStackTrace();
}
mobileRobot.setOutput(output);
}
private void scanInit() throws IOException {
currentPos();
scan("LASER");
currentPos();
scan("SONAR");
if (position[0] != startX && position[1] != startY) {
firstrun = false;
}
if (!firstrun) {
startX = mobileRobot.getPlatform().getRobotPosition().getX();
startY = mobileRobot.getPlatform().getRobotPosition().getY();
}
}
private void currentPos() throws IOException {
mobileRobot.sendCommand("R1.GETPOS");
result = input.readLine();
parsePosition(result, position);
}
private void moveForward(int steps) throws IOException {
mobileRobot.sendCommand("P1.MOVEFW " + steps * occupancyMap.getCellDimension());
result = input.readLine();
}