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 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;
}
/**
* 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();
}