public static void Measurement() {
// execute all actions of this state
LCD.drawString("Measurement", 0, 3);
btThread.popElement();
Sound.beep();
Sound.beep();
// lower sensor by RCX motor
tempMotor.setPower(-100);
tempMotor.setPower(0);
for (int i = 0; i < nrcolors; i++) {
if (colorsens.getColorID() == lakes[i].color && !lakes[i].found) // kleuren komen overeen
lakes[i].celsius = tempSensor.getCelcius();
lakes[i].found = true;
return;
}
// raise temp sensor
tempMotor.setPower(100);
btThread.write(500); // sends the message 'action done'
// leg de huidige tijd vast voor alle transitions met een timeoutcondition
long starttime = System.currentTimeMillis();
// when done, wait for a trigger for a transition
boolean transitionTaken = false;
while (!transitionTaken) {
if ((lakes[0].found && lakes[1].found && lakes[2].found)) {
current = State.FINISHED;
transitionTaken = true;
} else if ((true)) {
current = State.WATCH;
transitionTaken = true;
}
}
}
/**
* Calibrates a single axis. The calibration process consists of determining static acceleration
* values due to gravity by placing the axis opposite and in the direction of gravity. This gives
* a minimum and maximum value. The difference between the two is equal to 2G and used to
* calculate a scale factor. The mean of the two corresponds to 0G and equals the offset value.
*
* <p>Calibration settings are held in memory. To store these values one should use the <code>
* {@link #save}</code> method.
*
* @param axis Axis should have the value X,Y or Z
*/
public void calibrateAxis(char axis) {
float max = 0;
float min = 0;
float minG = 0;
float maxG = 0;
float oldOffset = 0;
float oldScale = 0;
int index = 0;
while (Button.ENTER.isDown()) ;
switch (axis) {
case 'X':
index = 0;
break;
case 'Y':
index = 1;
break;
case 'Z':
index = 2;
break;
default:
return;
}
oldOffset = offset[index];
oldScale = scale[index];
offset[index] = 0;
scale[index] = 1;
LCD.clear();
LCD.drawString("Point " + axis + " up.", 0, 1);
LCD.drawString("Press enter to start", 0, 2);
showLowPass(index);
LCD.drawString("Sampling... ", 0, 2);
Sound.beep();
max = getRawMean(index);
maxG = getMeanG(index);
Sound.beep();
LCD.clear();
LCD.drawString("Point " + axis + " down.", 0, 1);
LCD.drawString("Press enter to start", 0, 2);
showLowPass(index);
LCD.drawString("Sampling... ", 0, 2);
Sound.beep();
min = getRawMean(index);
minG = getMeanG(index);
Sound.beep();
LCD.clear();
LCD.drawString("offset range", 4, 0);
LCD.drawString("old", 0, 2);
LCD.drawString(Double.toString(oldOffset), 4, 2);
LCD.drawString(Double.toString(oldScale), 11, 2);
LCD.drawString("new", 0, 3);
offset[index] = (min + max) / 2.f;
scale[index] = 2.f / (maxG - minG);
LCD.drawString(Double.toString(offset[index]), 4, 3);
LCD.drawString(Double.toString(scale[index]), 11, 3);
LCD.drawString("Press enter", 0, 7);
Button.ENTER.waitForPressAndRelease();
}
public static void Finished() {
// execute all actions of this state
LCD.drawString("Finished", 0, 3);
Sound.beep();
Sound.beep();
Sound.beep();
btThread.write(300); // sends the message 'all done'
}
static void init() throws InterruptedException {
Sound.beep();
try {
File f = new File("CalibrationData.dat");
FileInputStream is = new FileInputStream(f);
colors = RescueColors.readObject(is);
is.close();
LCD.drawString("File read", 0, 0);
LCD.drawString("sucessfully", 0, 0);
} catch (IOException e) {
LCD.drawString("Error reading", 0, 0);
LCD.drawString("file", 0, 1);
Thread.sleep(1000);
System.exit(-1);
}
colors.printToLCD();
robot = new RescueRobot(colors);
Button.ENTER.waitForPressAndRelease();
logger = new MovementLogger(robot);
Thread t = new Thread(logger);
t.setDaemon(true);
t.start();
}
public static void main(String[] args) {
try {
Thread.setDefaultUncaughtExceptionHandler(new ThreadHandler());
Sound.playTone(2000, 300);
BtReceiver btReceiver = new BtReceiver();
btReceiver.connect();
} catch (Exception e) {
Sound.buzz();
System.out.println("Main: exception");
System.out.println(e.toString());
System.out.print(e.getMessage());
Utils.sleep(1000);
Button.waitForAnyPress();
System.exit(0);
}
}
public void run() {
System.out.println("Watching stalls");
while (!Thread.interrupted()) {
index = 0;
for (Motor m : motors) {
newSpeed = m.getActualSpeed();
if (m.isRotating()) {
if (newSpeed < limit) {
if (oldSpeed[index] > newSpeed || wasStalled[index]) {
if (oldSpeed[index] > newSpeed) {
Sound.beep();
}
stalled = true;
m.flt();
wasStalled[index] = false;
}
wasStalled[index] = true;
}
} else {
oldSpeed[index] = 0;
}
oldSpeed[index] = newSpeed;
index++;
// TODO: this will miss a chunk abut once every 20-50 (actually more, this probably takes
// at least 10ms)
try {
Thread.sleep(95);
} catch (InterruptedException e) {
}
}
}
}
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();
}
/**
* Initialise divers paramètre
*
* @see Param#VOLUME
* @see Param#SPEED_CRUISE
* @see Param#RSPEED_CRUISE
* @see Param#RSPEED_SONARMOTOR
*/
private void init(ThreadRobot tRobot) {
Sound.setVolume(VOLUME);
tRobot.getMov().getDiffPilot().setTravelSpeed(Movement.SPEED_CRUISE);
tRobot.getMov().getDiffPilot().setRotateSpeed(Movement.RSPEED_CRUISE);
tRobot.getSonarMotor().setSpeed(Movement.RSPEED_SONARMOTOR);
tRobot.getEnv().setInitValues(10, 10, 0); // A faire depuis la com
}
// The recognize() method returns true if the object is a blue block
public boolean recognize() {
// Boolean that determines whether or not to start detecting what kind of object is in front
boolean isRecognizing = true;
while (isRecognizing) {
// The robot will drive slowly if the light reads under 300, which means an object is closeby
while (color.getNormalizedLightValue() < 300) {
robot.setForwardSpeed(2);
}
// When the robot is close enough to the object, it will stop then turn on its
// blue light which gives two noticeable different light values for a wooden and blue
// block
robot.setForwardSpeed(0);
color.setFloodlight(Color.BLUE);
// If the light value of blue reads over 250, it is a blue block
if (color.getNormalizedLightValue() >= 250) {
Sound.beep();
LCD.drawString("Blue styrofoam block", 3, 5);
// Turn the default light back on after analyzing an object
color.setFloodlight(true);
return true;
} else { // If it is not a blue block, it is a wooden block
Sound.buzz();
LCD.drawString("Wooden block", 3, 5);
// Turn the default light back on after analyzing an object
color.setFloodlight(true);
}
// Set the boolean to false to break out of the while loop and stop analyzing the object
isRecognizing = false;
}
// Default return value. It should never get here but JAVA requires a return type
return false;
}
public void run() {
while (true) {
for (int i = 0; i < note1.length; i += 2) {
final int tone = (int) note1[i];
final int b = i + 1;
final int length = 10 * note1[b];
Sound.playTone(tone, length);
try {
Thread.sleep(length);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}
}
public static void Sonarflag() {
// execute all actions of this state
LCD.drawString("Sonarflag", 0, 3);
Sound.beep();
btThread.write(sonar.getDistance()); // sends the data from the sonar
// leg de huidige tijd vast voor alle transitions met een timeoutcondition
long starttime = System.currentTimeMillis();
// when done, wait for a trigger for a transition
boolean transitionTaken = false;
while (!transitionTaken) {
if ((true)) {
current = State.WATCH;
transitionTaken = true;
}
}
}
public static void main(String[] args) {
int option = 0;
while (option == 0) option = Button.waitForAnyPress();
MobileRobot robot = new MobileRobot();
MobileRobot.corr.turnOnLightSensors();
MobileRobot.corr.startCorrectionTimer();
robot.liftClaw();
robot.performRotationCorrectionLocalization();
// robot.travelCoordinate(0,0,true);
Sound.beep();
}
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();
}
public static boolean findline() {
TachoPilot p = robot.pilot;
int[] angles = new int[] {30, -30, 60, -60, 90, -90, 0};
int lastangle = 0;
for (int angle : angles) {
p.rotate((lastangle - angle) * 4, true);
while (p.isMoving()) {
if (linefound()) {
Sound.beepSequence();
p.stop();
return true;
}
Thread.yield();
}
p.stop();
lastangle = angle;
Delay.msDelay(250);
}
return false;
}
public void MyTune() {
// NOTE: This tune was generated from a midi using Guy
// Truffelli's Brick Music Studio www.aga.it/~guy/lego
short[] note = {
2349, 115, 0, 5, 1760, 165, 0, 35, 1760, 28, 0, 13, 1976, 23, 0, 18, 1760, 18, 0, 23, 1568,
15, 0, 25, 1480, 103, 0, 18, 1175, 180, 0, 20, 1760, 18, 0, 23, 1976, 20, 0, 20, 1760, 15, 0,
25, 1568, 15, 0, 25, 2217, 98, 0, 23, 1760, 88, 0, 33, 1760, 75, 0, 5, 1760, 20, 0, 20, 1760,
20, 0, 20, 1976, 18, 0, 23, 1760, 18, 0, 23, 2217, 225, 0, 15, 2217, 218
};
for (int i = 0; i < note.length; i += 2) {
final short w = note[i + 1];
final int n = note[i];
if (n != 0) Sound.playTone(n, w * 10);
try {
Thread.sleep(w * 2);
} catch (InterruptedException e) {
}
}
}
public void run() {
System.out.println("Connecting to " + name);
conn = Bluetooth.connect(Bluetooth.getKnownDevice(name));
conn.setIOMode(NXTConnection.PACKET);
dis = conn.openDataInputStream();
dos = conn.openDataOutputStream();
System.out.println("NXT connected");
// Request dimensions
try {
dos.writeByte(3);
dos.flush();
} catch (IOException e1) {
}
try {
while (true) {
byte type = dis.readByte();
switch (type) {
case 1:
byte slot = dis.readByte();
byte state = dis.readByte();
states[slot] = state;
break;
case 3:
slotCount = dis.readByte();
stateCount = dis.readByte();
break;
case 127:
EStop.thread.activate = true;
break;
}
}
} catch (IOException e) {
Sound.buzz();
System.out.println("Comm dropped to " + name);
}
}
public static void main(String... args) throws InterruptedException, IOException {
Thread display = new SoccerDisplay(robot);
display.start();
robot.pilot.setRegulation(true);
while (true) {
robot.pilot.setMoveSpeed(50);
while (!robot.IR.hasDirection()) {
robot.status = "Searching";
robot.pilot.travel(180);
Thread.sleep(100);
}
robot.pilot.setMoveSpeed(200);
robot.pilot.stop();
while (robot.IR.hasDirection()) {
while (robot.hasBall()) {
robot.status = "Dribbling";
robot.pilot.travel(0);
Sound.beepSequenceUp();
Thread.sleep(250);
robot.kick();
robot.pilot.stop();
Thread.sleep(500);
}
float angle = robot.IR.getAngle();
if (Math.abs(angle) > 60) {
// robot.status = "Behind";
// float angle2 = 180 - Math.signum(angle) * (180 - Math.abs(angle)) * 0.75f;
robot.status = "" + angle; // + ", " + angle2 + " ";
robot.pilot.travel(angle); // 2);
Thread.sleep(500);
} else if (!Float.isNaN(angle)) {
robot.pilot.travel(angle);
}
Thread.sleep(100);
}
}
}
public static void Colorflag() {
// execute all actions of this state
LCD.drawString("Colorflag", 0, 3);
Sound.beep();
btThread.write(400); // sends the message 'new color found'
// leg de huidige tijd vast voor alle transitions met een timeoutcondition
long starttime = System.currentTimeMillis();
// when done, wait for a trigger for a transition
boolean transitionTaken = false;
while (!transitionTaken) {
if ((starttime + 30000 <= System.currentTimeMillis())) {
current = State.WATCH;
transitionTaken = true;
} else if ((btThread.peekElement() == 500)) {
current = State.MEASUREMENT;
transitionTaken = true;
}
}
}
/**
* Loads saved offset and scale values from flash. If no saved values are found it will load
* default values for offset=0 and scale =1;
*/
public void load() {
File store = new File(name);
FileInputStream in = null;
if (store.exists()) {
try {
in = new FileInputStream(store);
DataInputStream din = new DataInputStream(in);
for (int i = 0; i < 3; i++) {
offset[i] = din.readFloat();
scale[i] = din.readFloat();
}
din.close();
} catch (IOException e) {
System.err.println("Failed to load calibration");
Sound.beep();
}
} else {
for (int i = 0; i < 3; i++) {
offset[i] = 0;
scale[i] = 1;
}
}
}
/** Fonction d'initialisation test */
public static void initTest(ThreadRobot tRobot) {
Sound.setVolume(VOLUME);
tRobot.getMov().getDiffPilot().setTravelSpeed(Movement.SPEED_CRUISE);
tRobot.getMov().getDiffPilot().setRotateSpeed(Movement.RSPEED_CRUISE);
tRobot.getSonarMotor().setSpeed(Movement.RSPEED_SONARMOTOR);
tRobot.getEnv().setInitValues(10, 10, 0);
tRobot.getOrder().add(Order.WAITBUTTON);
tRobot.getOrder().chooseInsecurely();
tRobot.getOrder().execute();
tRobot.getOrder().add(Order.WAIT1SEC);
tRobot.getOrder().chooseInsecurely();
tRobot.getOrder().execute();
tRobot.getOrder().add(Order.SETPOSITION);
tRobot.getOrder().chooseInsecurely();
tRobot.getOrder().execute();
tRobot.getOrder().add(Order.SAVEREFANGLE);
tRobot.getOrder().chooseInsecurely();
tRobot.getOrder().execute();
tRobot.getOrder().add(Order.CHECKFIRSTCASE);
tRobot.getOrder().chooseInsecurely();
tRobot.getOrder().execute();
}
/** Runs the ai... */
public void run() {
Communicator comm = null;
try {
comm = Communicator.getInstance(this);
} catch (Exception e) {
Console.println("-------------------");
Console.println("ERROR in Comm");
Console.println("-------------------");
Button.waitForPress();
return;
}
while (grid.isWorkToDo()) {
PathElement p = null;
if (motion.isObjLoaded()) {
p = grid.getAWayBackHome(robo);
} else if ((p = grid.getAWayToNextKnownUnCommon(robo)) == null) {
if ((p = grid.getAWayToNextUnknown(robo)) == null) {
/* does not work..
if ((p = grid.getAWayToNextKnownCommon(robo)) == null) {
try {
sleep(500);
} catch (InterruptedException e) {
// can't sleep, so do a busy waiting
}
continue; // try again
}*/
}
}
Console.println("here3"); // TODO
do {
orient(p);
// ask other
if (!askOther(p)) continue; // try later again
// if its not for me, i won't go there. (see grid.getway...)
if (grid.getJunction(p).getType() == Junction.MASTER_OBJ
|| grid.getJunction(p).getType() == Junction.SLAVE_OBJ) {
// load
motion.goToNextJunction(true);
grid.setJunction(new Junction(robo.getMyActualPosition(), Junction.EMPTY), true);
} else {
// unloading loaded object
if (motion.isObjLoaded()
&& grid.getJunction(grid.getNextProjectedPosition(robo)).getType()
== Junction.HOME_BASE) {
motion.goToNextJunction(false);
// orient to south
Position orientation = grid.getNextProjectedPosition(robo);
Position unloadPos =
new Position(robo.getHomeBase().getX(), robo.getHomeBase().getY() - 1);
// unload the object
orient(unloadPos);
motion.unload();
// orient back
orient(orientation);
// regular move
} else motion.goToNextJunction(false);
}
} while ((p = p.getNextElt()) != null);
}
Console.println(" - FIN - ");
Sound.playTone(400, 1000);
Button.waitForPress();
}
/**
* Plays a sound file once from the NXT. SoundSensor files use the .rso extension. The filename is
* not case sensitive. Filenames on the NXT Bricks display do now show the filename extension.
*
* @param fileName e.g. "Woops.rso"
* @return If you receive a non-zero number, the filename is probably wrong or the file is not
* uploaded to the NXT brick.
*/
public static byte playSoundFile(String fileName) {
return Sound.playSoundFile(fileName, false);
}
/** 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();
}
}
}
/**
* Localizes the robot using the ultrasonic sensor. Clocks the angle at which the sensor detects
* each wall and calculates 0 degrees
*/
public void doLocalization() {
angleA = 0;
angleB = 0;
int UCdistance, expmtDistance;
expmtDistance = 38;
int count = 0;
boolean facingToWall = true;
// rotate the robot until it sees no wall
nav.rotate(165, -165);
facingToWall = true;
while (facingToWall) {
UCdistance = getFilteredData();
if (UCdistance > expmtDistance) {
count++;
}
if (UCdistance > expmtDistance && count >= 3) {
facingToWall = false;
count = 0;
}
}
sleep(3000);
// keep rotating until the robot sees a wall, then latch the angle
while (!facingToWall) {
UCdistance = getFilteredData();
if (UCdistance < expmtDistance) {
count++;
}
if (UCdistance < expmtDistance && count >= 3) {
facingToWall = true;
angleA = odo.getTheta();
Sound.beep();
nav.stopMotor();
count = 0;
}
}
// switch direction and wait until it sees no wall
nav.rotate(-165, 165);
while (facingToWall) {
UCdistance = getFilteredData();
if (UCdistance > expmtDistance) {
count++;
}
if (UCdistance > expmtDistance && count >= 3) {
facingToWall = false;
count = 0;
}
}
// keep rotating until the robot sees a wall, then latch the angle
sleep(3000);
while (!facingToWall) {
UCdistance = getFilteredData();
if (UCdistance < expmtDistance) {
count++;
}
if (UCdistance < expmtDistance && count >= 3) {
facingToWall = true;
angleB = odo.getTheta();
Sound.beep();
nav.stopMotor();
}
}
// angleA is clockwise from angleB, so assume the average of the
// angles to the right of angleB is 45 degrees past 'north'
// we slightly correct the angle '45' to '44' based on the error we measured
angle = 45 - (angleA - angleB) / 2;
// update the odometer position (example to follow:)
odo.setTheta(angle);
nav.turnTo(0);
}