private int[] searchWall(int xCoord, int yCoord, int previouseX, int previouseY) {
int[] nextWall = new int[2];
char[][] map = occupancyMap.getGrid();
if (xCoord > 0
&& map[xCoord - 1][yCoord] == occupancyMap.getObstacle()
&& (xCoord - 1 != previouseX || yCoord != previouseY)) {
nextWall[0] = xCoord - 1;
nextWall[1] = yCoord;
} else if (map[xCoord + 1][yCoord] == occupancyMap.getObstacle()
&& (xCoord + 1 != previouseX || yCoord != previouseY)) {
nextWall[0] = xCoord + 1;
nextWall[1] = yCoord;
} else if (yCoord > 0
&& map[xCoord][yCoord - 1] == occupancyMap.getObstacle()
&& (xCoord != previouseX || yCoord - 1 != previouseY)) {
nextWall[0] = xCoord;
nextWall[1] = yCoord - 1;
} else if (map[xCoord][yCoord + 1] == occupancyMap.getObstacle()
&& (xCoord != previouseX || yCoord + 1 != previouseY)) {
nextWall[0] = xCoord;
nextWall[1] = yCoord + 1;
} else {
throw new IllegalArgumentException("");
}
return nextWall;
}
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 boolean searchRightWall(int xCoord, int yCoord) {
boolean rightWall = true;
int[] lookingDirection = lookingDirection(RIGHT);
int xLooking = lookingDirection[0];
int yLooking = lookingDirection[1];
char[][] map = occupancyMap.getGrid();
boolean end = false;
int predictedStepsWall = 0;
while (predictedStepsWall < 6 && !end) {
if (map[xCoord][yCoord] == occupancyMap.getUnknown()) {
end = true;
rightWall = false;
} else if (map[xCoord][yCoord] == occupancyMap.getObstacle()) {
end = true;
rightWall = true;
}
predictedStepsWall++;
xCoord += xLooking;
yCoord += yLooking;
}
if (!end) {
rightWall = false;
}
return rightWall;
}
private boolean scannedMap() {
boolean scannedMap = false;
int xCoord = (int) position[0] / occupancyMap.getCellDimension();
int yCoord = (int) position[1] / occupancyMap.getCellDimension();
if (searchRightWall(xCoord, yCoord)) {
int[] search = lookingDirection(90);
int xSearch = search[0];
int ySearch = search[1];
char[][] map = occupancyMap.getGrid();
while (map[xCoord][yCoord] != occupancyMap.getObstacle()) {
xCoord += xSearch;
yCoord += ySearch;
}
boolean continued = true;
int startX = xCoord;
int startY = yCoord;
int currentX = xCoord;
int currentY = yCoord;
int previouseX = startX;
int previouseY = startY;
do {
try {
int[] wallCoords = searchWall(currentX, currentY, previouseX, previouseY);
previouseX = currentX;
previouseY = currentY;
currentX = wallCoords[0];
currentY = wallCoords[1];
} catch (IllegalArgumentException a) {
continued = false;
}
} while ((currentX != startX || currentY != startY) && continued);
if (continued) {
scannedMap = true;
}
}
return scannedMap;
}
private void rightCorner(int predictedSteps) throws IOException {
moveForward(predictedSteps + 3);
scanInit();
int xCoord = (int) position[0] / occupancyMap.getCellDimension();
int yCoord = (int) position[1] / occupancyMap.getCellDimension();
boolean rightWall = searchRightWall(xCoord, yCoord);
if (!rightWall) {
rotate("RIGHT");
moveForward(2);
}
}
private void processScan() throws IOException {
scanInit();
int xCoord = (int) position[0] / occupancyMap.getCellDimension();
int yCoord = (int) position[1] / occupancyMap.getCellDimension();
int[] lookingDirection = lookingDirection(FORWARD);
int xLooking = lookingDirection[0];
int yLooking = lookingDirection[1];
boolean end = false;
char[][] map = occupancyMap.getGrid();
int predictedSteps = 0;
while (predictedSteps < MAX_VIEW && !end) {
boolean rightWall = searchRightWall(xCoord, yCoord);
if (rightWall && map[xCoord][yCoord] == occupancyMap.getEmpty()) {
predictedSteps++;
} else if (rightWall && map[xCoord][yCoord] == occupancyMap.getUnknown()) {
moveForward(predictedSteps - 2);
end = true;
} else if (rightWall && map[xCoord][yCoord] == occupancyMap.getObstacle()) {
moveForward(predictedSteps - 3);
rotate("LEFT");
end = true;
} else if (!rightWall && map[xCoord][yCoord] == occupancyMap.getUnknown()) {
moveForward(predictedSteps - 2);
end = true;
} else if (!rightWall && map[xCoord][yCoord] == occupancyMap.getEmpty()) {
rightCorner(predictedSteps);
end = true;
}
xCoord += xLooking;
yCoord += yLooking;
}
if (!end) {
moveForward(predictedSteps - 2);
}
}
private void moveForward(int steps) throws IOException {
mobileRobot.sendCommand("P1.MOVEFW " + steps * occupancyMap.getCellDimension());
result = input.readLine();
}