private static Trajectory searchTile() {
UltrasonicPoller up = UltrasonicPoller.getInstance();
NXTRegulatedMotor sensorMotor = Configuration.SENSOR_MOTOR;
sensorMotor.setSpeed(45);
sensorMotor.rotateTo(-55, true);
// LCDWriter.getInstance().writeToScreen("D: " + up.getDistance(), 0);
// TODO: better algorithm
while (sensorMotor.getPosition() > -55) {
LCDWriter.getInstance().writeToScreen("D: " + up.getDistance(), 0);
if (up.getDistance() < 25) {
sensorMotor.rotateTo(0);
return new Trajectory(-sensorMotor.getPosition() + 15, up.getDistance());
}
}
sensorMotor.rotateTo(55, true);
while (sensorMotor.getPosition() < 55) {
LCDWriter.getInstance().writeToScreen("D: " + up.getDistance(), 0);
if (up.getDistance() < 25) {
// sensorMotor.stop();
sensorMotor.rotateTo(0);
return new Trajectory(-sensorMotor.getPosition() - 15, up.getDistance());
}
}
return null;
}
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();
}
/**
* test the distance measured by the us sensor and see if it's good
*
* @author yuechuan
*/
public class USDTest {
LCDWriter lcd = LCDWriter.getInstance();
UltrasonicPoller up = UltrasonicPoller.getInstance();
public static void main(String[] args) {
USDTest usdt = new USDTest();
usdt.lcd.start();
usdt.up.start();
while (true) {
usdt.lcd.writeToScreen(usdt.up.getDistance() + "", 0);
try {
Thread.sleep(100);
} catch (Exception e) {
}
}
}
}
public static void main(String[] args) {
LCDWriter lcd = LCDWriter.getInstance();
lcd.start();
UltrasonicPoller up = UltrasonicPoller.getInstance();
Odometer odo = Odometer.getInstance();
Driver.setSpeed(30);
Driver dr = Driver.getInstance();
ObjRec or = new ObjRec();
up.start();
odo.start();
Trajectory block = searchTile();
//
lcd.writeToScreen("here", 1);
if (block != null) {
dr.turnTo(odo.getTheta() + block.theta);
dr.forward(Math.abs(block.distance - 5));
try {
// if(or == null) lcd.writeToScreen("NO!", 1);
ArrayList<ObjRec.blockColor> color = or.detect();
//
if (color == null || color.size() == 0) lcd.writeToScreen("EMPTY", 1);
else {
int count = 1;
for (ObjRec.blockColor c : color) if (c != null) lcd.writeToScreen(c.name(), count++);
}
} catch (Exception e) {
lcd.writeToScreen("EXCEPTION", 1);
}
}
// lcd.writeToScreen("fin", 1);
// indicate finish
Sound.beepSequenceUp();
}
/** 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();
}
}
}
