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