/**
* Am I in the search zone?
*
* @return
*/
public static boolean inSearchZone() {
Coordinate now = Configuration.getInstance().getCurrentLocation();
Coordinate[] bl = Configuration.getInstance().getFlagZone();
return (now.getX() > bl[0].getX()
&& now.getX() < bl[1].getX()
&& now.getY() > bl[0].getY()
&& now.getY() < bl[1].getY())
&& !nearWall();
}
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;
}
/**
* Get the point from an arraylist closest to the given coordinate
*
* @param a
* @param b
* @return
*/
public static Coordinate getClosestPoint(Coordinate a, ArrayList<Coordinate> b) {
double bestDist = Integer.MAX_VALUE;
Coordinate best = null;
for (Coordinate coord : b) {
double current = Coordinate.calculateDistance(coord, a);
if (current < bestDist) {
bestDist = current;
best = coord;
}
}
if (best != null) b.remove(best);
return best;
}
/** Run a search routine */
public static void searchForBlock() {
Stack<Coordinate> path = Searcher.generateSearchPath();
Configuration conf = Configuration.getInstance();
Driver driver = Driver.getInstance();
Coordinate first = path.peek();
LCDWriter lcd = LCDWriter.getInstance();
CaptureMechanism cm = CaptureMechanism.getInstance();
int BLOCK_COLOR = conf.getBlockColor().getCode();
boolean blockFound = false;
Configuration.getInstance().setForwardSpeed(300);
Configuration.getInstance().setRotationSpeed(250);
while (!blockFound && !path.isEmpty()) {
Coordinate p = path.pop();
lcd.writeToScreen("Des:" + p.toString(), 4);
driver.turnTo(Coordinate.calculateRotationAngle(conf.getCurrentLocation(), p));
int result = ObjectDetector.lookForItemII(BLOCK_COLOR);
if (result == 1) {
Sound.beep();
blockFound = true;
cm.open();
driver.forward(15);
cm.align();
driver.forward(15);
cm.close();
Sound.beep();
Sound.beepSequenceUp();
break;
}
// If not the right block remove it
else if (result == 0 && Searcher.inSearchZone()) {
cm.removeBlockII();
ObjRec oRec = new ObjRec();
// test again
ArrayList<blockColor> bc = oRec.detect();
String ans = "";
for (blockColor b : bc) {
ans += b.toString();
}
lcd.writeToScreen(ans, 6);
if (bc.contains(blockColor.getInstance(BLOCK_COLOR))) {
// face the robot
blockFound = true;
cm.open();
driver.forward(15);
cm.align();
driver.forward(15);
cm.close();
Sound.beep();
Sound.beepSequenceUp();
break;
}
}
driver.travelTo(p);
// Double check for item
result = ObjectDetector.lookForItemII(BLOCK_COLOR);
// capture it
if (result == 1) {
Sound.beep();
blockFound = true;
cm.open();
driver.forward(15);
cm.align();
driver.forward(15);
cm.close();
Sound.beep();
Sound.beepSequenceUp();
break;
} else if (result == 0 && Searcher.inSearchZone()) {
cm.removeBlockII();
ObjRec oRec = new ObjRec();
// test again
ArrayList<blockColor> bc = oRec.detect();
String ans = "";
for (blockColor b : bc) {
ans += b.toString();
}
lcd.writeToScreen(ans, 6);
if (bc.contains(blockColor.getInstance(BLOCK_COLOR))) {
// face the robot
blockFound = true;
cm.open();
driver.forward(15);
cm.align();
driver.forward(15);
cm.close();
Sound.beep();
Sound.beepSequenceUp();
break;
}
}
if (path.isEmpty() && !blockFound) {
path = Searcher.generateSearchPath();
}
}
}
/**
* Is the robot near a wall?
*
* @return
*/
public static boolean nearWall() {
Coordinate now = Configuration.getInstance().getCurrentLocation();
return now.getX() < 25 || now.getY() < 25 || now.getX() > 305 || now.getY() > 305;
}
