System.out.println("Located: (" + (int) start.getX() + "," + (int) start.getY() + ")");
// Go home
try {
Collection<WayPoint> route = pf.findRoute(start, home);
for(WayPoint wp: route) {
System.out.println("Go to (" + (int) wp.x + "," + (int) wp.y + ")");
Pose pose = pc.goTo(wp);
goodEstimate(pose); // Just for diagnostics