// Method scans input and maps it; if obstacle is in front, left and right,
// backpedal. If there is a space adjacent but an obstacle at the front,
// turns to move around the obstacle using the available space.
public static void movRow() {
double nextCell = mapObj.basicProb(); // work out probability
// output to RConsole through BlueTooth
// btObj.stringToRCon("Object Probability in next Cell: %2f" + nextCell);
// mapObj.printMap(columns, numOfRowCells);
// sonar scan around of the robot
boolean[] scanResults = new boolean[3];
scanResults = mapObj.scanAll();
mapObj.updateMap(scanResults);
// btObj.stringToRCon("Scan Results: " + scanResults[0] + ", " + scanResults[1] + ", " +
// scanResults[2]);
// mapObj.printMap(columns, numOfRowCells);
if (scanResults[1]) { // if there's an obstacle in front
// if there's an obstacle to the left & right. i.e. a dead end
if (scanResults[0] && scanResults[2]) {
turnAround(scanResults[2], mapObj, movObj); // move backwards
} else {
// call movAround() method to navigate around the obstacle
movAround(scanResults[1], mapObj, movObj);
}
} else {
movObj.nextCell(mapObj); // move forward a cell
}
}
// Method to move around an obstacle - runs when obstacle is in front
public static void movAround(boolean r, MapSystem ms, Movement mv) {
mv.turn(r, ms); // true = right turn; false = left turn
movRow(); // scan and move forward if necessary
mv.turn(!r, ms); // invert the boolean to turn the other way
// move forward twice to pass the obstacle
movRow();
++loop2;
movRow();
++loop2;
mv.turn(!r, ms); // turn to face original column
boolean extend = mapObj.scanAhead(); // flag to check object is passed
while (extend) {
mv.turn(r, ms); // turn to move along the axis
movObj.nextCell(mapObj);
++loop2;
mv.turn(!r, ms); // face the original column
if (!mapObj.scanAhead()) { // if an obstacle is not found anymore
extend = false; // break the while loop
} else {
}
}
movRow(); // move into the original column
mv.turn(r, ms); // face the correct way to continue the patrol
}
// method runs when robot is in a dead end and has to back up and go around
public static void turnAround(boolean r, MapSystem ms, Movement mv) {
mv.turn(ms); // 180 degree turn
movRow(); // move forward
// check for empty adjacent space
boolean[] scanResults = new boolean[3];
scanResults = mapObj.scanAll();
// Move to said empty space
if (scanResults[0] && !scanResults[2]) { // Obstacle on the Left
mv.turn(scanResults[0], ms); // turn right
movRow();
} else { // Obstacle on the Right
mv.turn(!scanResults[2], ms); // turn left
movRow();
mv.turn(!scanResults[2], ms);
movRow();
// move forward 2 cells, scanning as we go
// if an obstacle is still on the same axis as the first
// class it as a different obstacle
// move forward a cell
// scan again - recursive
// move back to the correct column
// if at a 'limit cell', turn left instead of right
}
}
public static void main(String[] args) {
// grid values for mapping
columns = 4;
numOfRowCells = 5;
cellSize = 30;
// connect to other classes
mapObj = new MapSystem(cellSize, columns, numOfRowCells);
movObj = new Movement(cellSize, columns, numOfRowCells);
// connect to RConsole
btObj.startBtConn();
System.out.println("Press Any Button");
Button.waitForAnyPress();
LCD.clear();
// for each column
for (loop1 = 0; loop1 < columns; loop1++) {
// for each cell within the column
for (loop2 = 0; loop2 < numOfRowCells; loop2++) {
movRow(); // scan and move method
}
if (loop1 == (columns - 1)) {
break;
}
if (loop1 % 2 == 0) { // if the robot moves along the Y axis positively
movCol(true); // turn right into the new column
} else {
movCol(false); // turn left into the new column
}
}
String[] output = mapObj.getMap(columns, numOfRowCells);
Button.waitForAnyPress();
btObj.stringToRCon(output[0]);
btObj.stringToRCon(output[1]);
btObj.stringToRCon(output[2]);
}