public static void main(String[] args) {
LCDWriter lcd = LCDWriter.getInstance();
lcd.start();
UltrasonicPoller up = UltrasonicPoller.getInstance();
PathTraveller traveller = PathTraveller.getInstance();
Odometer odo = Odometer.getInstance();
Driver dr = Driver.getInstance();
up.start();
odo.start();
traveller.recalculatePathToCoords(135, 105);
// traveller.popNextWaypoint();
// lcd.writeToScreen(str, lineNumber);
// while(!traveller.followThePath()){
// traveller.recalculatePathToCoords(75, 75);
// }
boolean done = false;
while (!done) {
done = followPath();
try {
if (!done) traveller.recalculatePathToCoords(135, 105);
else break;
} catch (Exception e) {
lcd.writeToScreen("E: " + e.toString(), 1);
}
}
// indicate finish
Sound.beepSequenceUp();
}
private static boolean followPath() {
PathTraveller t = PathTraveller.getInstance();
Driver driver = Driver.getInstance();
ObstacleDetector detector = ObstacleDetector.getInstance();
Map map = Map.getInstance();
while (!t.pathIsEmpty()) {
Waypoint next = t.popNextWaypoint();
// Turn to the next tile
driver.turnTo(
Coordinate.calculateRotationAngle(
Odometer.getInstance().getCurrentCoordinate(), new Coordinate(next)));
// scan the next area
if (detector.scanTile()) {
// block next tile
map.blockNodeAt(next.x, next.y);
return false;
}
driver.travelTo(new Coordinate(next));
}
return true;
}
/** Generates a search path based on the config */
public static Stack<Coordinate> generateSearchPath() {
ArrayList<Coordinate> points = PathTraveller.getInstance().getAllPointsInFlagZoneList();
Stack<Coordinate> searchpath = new Stack<Coordinate>();
Coordinate latest = getClosestPoint(Odometer.getInstance().getCurrentCoordinate(), points);
searchpath.push(latest);
while (points.size() > 0) {
latest = getClosestPoint(latest, points);
searchpath.push(latest);
}
Stack<Coordinate> temp = new Stack<Coordinate>();
while (!searchpath.isEmpty()) {
temp.push(searchpath.pop());
}
return temp;
}