@Override
public void action() {
boolean exit = false;
// LCD.clear();
LCD.drawString("Press UP for Menu", 0, 0);
LCD.drawString("Press DOWN for EXIT", 0, 1);
this.hal.stop();
while (!exit) {
Delay.msDelay(50);
if (Button.UP.isDown()) {
// Sound.beep();
// this lets the arbitrator exit and the main while-loop continues
sharedState.setState(MyState.ExitState);
exit = true;
} else if (Button.DOWN.isDown()) {
this.hal.destroy();
System.exit(0);
}
}
}
@Override
public void suppress() {
LCD.drawString("RIGHT suppress", 0, 5); // for debugging later
turn_right = false;
sharedPilot.robot.stop();
}
private void followLine(boolean startChainBridge) {
this.lineFollowing = new LineFollowing(drive, colorSensor);
lineFollowing.run(startChainBridge);
// Start search for barcode.
if (RACE_MODE && PROGRAM_FINISHED_START_BARCODE) {
PROGRAM_FINISHED_START_BARCODE = false;
LCD.clear();
System.out.println("Mode: Barcode");
barcode(true);
} else if (RACE_MODE && PROGRAM_STATUS == PROGRAM_BRIDGE) {
PROGRAM_CHANGED = false;
LCD.clear();
System.out.println("Mode: Bridge");
bridge();
}
}
private void elevatorCollision() {
LCD.clear();
LCD.drawString("Collision: " + collision, 0, 4);
Delay.msDelay(3000);
float[] dist = new float[sonicSensor.getDistanceMode().sampleSize()];
sonicSensor.getDistanceMode().fetchSample(dist, 0);
if ((dist[0] - DISTANCE_TO_WALL) > 0.005) {
drive.turnRight(5, false);
} else {
drive.turnLeft(5, false);
}
if (collision == "Wall") {
drive.stop();
} else if (collision == "Left Wall") {
drive.turnRight(5, false);
} else if (collision == "Right Wall") {
drive.moveDistance(drive.maxSpeed(), -2);
drive.turnLeft(20, false);
}
}
/*
* Initializing the bridge mode.
*/
private void bridge() {
this.bridge = new Bridge(drive, sonicMotor, leftMotor, rightMotor, sonicSensor, colorSensor);
bridge.run();
// Change bridge -> elevator
if (RACE_MODE && PROGRAM_CHANGED && PROGRAM_STATUS == PROGRAM_ELEVATOR) {
PROGRAM_CHANGED = false;
LCD.clear();
System.out.println("Mode: Elevator");
elevator();
}
}
/*
* Initializing the seesaw mode.
*/
private void seesaw() {
this.seesaw = new Seesaw(drive, colorSensor);
seesaw.run();
// Start search for barcode.
if (RACE_MODE && PROGRAM_FINISHED_START_BARCODE) {
PROGRAM_FINISHED_START_BARCODE = false;
LCD.clear();
System.out.println("Mode: Barcode");
barcode(true);
}
}
/*
* Initializing the roll mode.
*/
private void rolls() {
this.rolls = new Rolls(drive, sonicSensor, sonicMotor, colorSensor);
rolls.run();
// Start search for barcode.
if (RACE_MODE && PROGRAM_FINISHED_START_BARCODE) {
PROGRAM_FINISHED_START_BARCODE = false;
LCD.clear();
System.out.println("Mode: Barcode");
barcode(true);
}
}
/*
* Initializing the final spurt.
*/
private void finalSpurt() {
FinalSpurt finalSpurt =
new FinalSpurt(
drive, sonicSensor, touchLeftSensor, touchRightSensor, sonicMotor, colorSensor);
finalSpurt.run();
// Change final spurt -> final boss
if (RACE_MODE && PROGRAM_CHANGED && PROGRAM_STATUS == PROGRAM_FINAL_BOSS) {
PROGRAM_CHANGED = false;
LCD.clear();
System.out.println("Mode: Final Boss");
finalBoss();
}
}
/*
* Initializing the elevator mode.
*/
private void elevator() {
this.elevator =
new Elevator(
drive, colorSensor, touchLeftSensor, touchRightSensor, sonicSensor, sonicMotor);
elevator.run();
// Start search for barcode.
if (RACE_MODE && PROGRAM_FINISHED_START_BARCODE) {
PROGRAM_FINISHED_START_BARCODE = false;
LCD.clear();
System.out.println("Mode: Barcode");
barcode(true);
}
}
/*
* Start program to read a barcode. If a valid barcode could be found the
* next program will be loaded.
*/
private void barcode(boolean moveRobot) {
this.barcode = new Barcode(drive, colorSensor, moveRobot);
barcode.run();
if (barcode != null) {
int foundBarcode = barcode.getBarcode();
LCD.clear();
// System.out.println("Barcode: " + foundBarcode);
if (foundBarcode != -1) {
// Change the current program if a valid barcode has been found
changeProgram(foundBarcode);
}
}
}
/** Initializes the main menu that enables the user to select a certain obstacle mode. */
public GUI() {
LCD.clear(); // Make sure display is clear before the menu is displayed
// Stop current obstacle program if the left button is pressed
Button.LEFT.addKeyListener(
new KeyListener() {
@Override
public void keyPressed(Key k) {
endAllPrograms();
}
@Override
public void keyReleased(Key k) {}
});
// Stop program when the escape button is pressed on the ev3 brick
Button.ESCAPE.addKeyListener(
new KeyListener() {
@Override
public void keyPressed(Key k) {
drive.stop();
endAllPrograms();
// Start the GUI again => main menu should be shown
// when the obstacle program has been interrupted
startGUI();
}
@Override
public void keyReleased(Key k) {}
});
startGUI();
}
private void displayMessage() {
LCD.drawString(botMessage, xPosition, yPosition);
Delay.msDelay(waitTime);
}
private void displayMessage(String message) {
LCD.drawString(message, xPosition, yPosition);
Delay.msDelay(waitTime);
LCD.clear();
}
public static void main(String[] args) {
int buttonChoice;
// setup the odometer and display
Odometer odo = new Odometer(leftMotor, rightMotor, 30, true);
final TextLCD t = LocalEV3.get().getTextLCD();
do {
// clear the display
t.clear();
// ask the user whether he wants to detect blocks or search blocks
t.drawString("< Left |Right >", 0, 0);
t.drawString(" | ", 0, 1);
t.drawString("Detect|Search ", 0, 2);
buttonChoice = Button.waitForAnyPress();
while (buttonChoice != Button.ID_LEFT
&& buttonChoice != Button.ID_RIGHT
&& buttonChoice != Button.ID_ESCAPE) {
/*
* These two if statements is to make the motor attached to the USsensor rotate
* 90 degrees before the main methods are launched
*/
if (buttonChoice == Button.ID_UP) {
Scan scan = new Scan(usMotor);
scan.usMotorSpeed(50);
scan.turnSensor(90);
buttonChoice = Button.waitForAnyPress();
}
if (buttonChoice == Button.ID_DOWN) {
Scan scan = new Scan(usMotor);
scan.usMotorSpeed(50);
scan.turnSensor(-90);
buttonChoice = Button.waitForAnyPress();
}
}
} while (buttonChoice != Button.ID_LEFT
&& buttonChoice != Button.ID_RIGHT
&& buttonChoice != Button.ID_ESCAPE);
if (buttonChoice == Button.ID_ESCAPE) {
System.exit(0);
}
SensorModes usSensor = new EV3UltrasonicSensor(usPort);
SampleProvider usValue =
usSensor.getMode("Distance"); // colorValue provides samples from this instance
float[] usData =
new float[usValue.sampleSize()]; // colorData is the buffer in which data are returned
SensorModes colorSensor = new EV3ColorSensor(colorPort);
SampleProvider colorValue =
colorSensor.getMode("ColorID"); // colorValue provides samples from this instance
float[] colorData =
new float[colorValue.sampleSize()]; // colorData is the buffer in which data are returned
// The following start the PartA of the Lab when the right button is pressed, afterwards press
// escape to exit program
while (buttonChoice != Button.ID_RIGHT && buttonChoice != Button.ID_ESCAPE) {
if (buttonChoice == Button.ID_LEFT) {
ObjectDetection od = new ObjectDetection(colorValue, colorData, usValue, usData);
od.run();
LCD.drawString("< Left |Right >", 0, 0);
LCD.drawString(" | ", 0, 1);
LCD.drawString("Detect|Search ", 0, 2);
}
buttonChoice = Button.waitForAnyPress();
}
if (buttonChoice == Button.ID_ESCAPE) {
System.exit(0);
}
// If the left button is pressed, the robot will start partB of the lab which is localize, and
// start scanning the field
odo.start();
final USLocalizer usl =
new USLocalizer(
odo,
usSensor,
usData,
USLocalizer.LocalizationType.FALLING_EDGE,
leftMotor,
rightMotor,
WHEEL_RADIUS,
TRACK);
final Scan scan =
new Scan(usValue, usData, colorValue, colorData, odo, leftMotor, rightMotor, usMotor);
br = new BlockRecognition(odo, usSensor, usData, colorValue, colorData, rightMotor, leftMotor);
new LCDInfo(odo, usSensor, usData, colorSensor, colorData);
// begin the threads (we launch a thread to be able to exit it whenever we want using escape)
(new Thread() {
public void run() {
br.start();
scan.start();
usl.doLocalization();
scan.startRun();
}
})
.start();
while (Button.waitForAnyPress() != Button.ID_ESCAPE) ;
System.exit(0);
}
private void displayMessage(String input) {
LCD.drawString(input, xPosition, yPosition);
Delay.msDelay(waitTime);
}
/*
* Helper method to initialize the GUI/creating the main menu.
*/
private void startGUI() {
// Creating the menu to select certain robot states/obstacles.
while (true) {
// The elements of the menu to display on the ev3 brick
String[] viewItems = {
"Labyrinth",
"Linie folgen",
"Bruecke",
"Haengebruecke",
"Rollen",
"Wippe",
"Aufzug",
"Endspurt",
"Endgegner",
"Exit",
"Barcode"
};
TextMenu menu = new TextMenu(viewItems, 1);
// LCD.clear();
int selection = menu.select();
/*
* Menu selection. Selection number is the index of the element in
* the viewItems array.
*/
if (selection == -1) {
// ESCAPE button pressed. End loop
break;
} else if (selection == 0) {
// Maze
LCD.clear();
LCD.drawString("Mode: Labyrinth", 0, 0);
PROGRAM_STATUS = PROGRAM_MAZE;
maze();
} else if (selection == 1) {
// Follow line
LCD.clear();
LCD.drawString("Mode: Linie folgen", 0, 0);
PROGRAM_STATUS = PROGRAM_FOLLOW_LINE;
followLine(false);
} else if (selection == 2) {
// Bridge
LCD.clear();
LCD.drawString("Mode: Bruecke", 0, 0);
PROGRAM_STATUS = PROGRAM_BRIDGE;
bridge();
} else if (selection == 3) {
// Chain bridge
LCD.clear();
LCD.drawString("Mode: Haengebruecke", 0, 0);
PROGRAM_STATUS = PROGRAM_CHAIN_BRDIGE;
chainBridge();
} else if (selection == 4) {
// Rolls
LCD.clear();
LCD.drawString("Mode: Rollen", 0, 0);
PROGRAM_STATUS = PROGRAM_ROLLS;
rolls();
} else if (selection == 5) {
// Seesaw
LCD.clear();
LCD.drawString("Mode: Wippe", 0, 0);
PROGRAM_STATUS = PROGRAM_SEESAW;
// seesaw();
followLine(false);
} else if (selection == 6) {
// Elevator
LCD.clear();
LCD.drawString("Mode: Aufzug", 0, 0);
PROGRAM_STATUS = PROGRAM_ELEVATOR;
elevator();
} else if (selection == 7) {
// Final spurt
LCD.clear();
LCD.drawString("Mode: Endspurt", 0, 0);
PROGRAM_STATUS = PROGRAM_FINAL_SPURT;
finalSpurt();
} else if (selection == 8) {
// Final boss
LCD.clear();
LCD.drawString("Mode: Endgegner", 0, 0);
PROGRAM_STATUS = PROGRAM_FINAL_BOSS;
finalBoss();
} else if (selection == 9) {
// Terminate whole program on ev3 brick
LCD.clear();
PROGRAM_STATUS = PROGRAM_EXIT;
System.exit(0);
} else if (selection == 10) {
// Barcode
LCD.clear();
LCD.drawString("Mode: Barcode", 0, 0);
PROGRAM_STATUS = PROGRAM_BARCODE;
barcode(true);
}
PROGRAM_STATUS = -1;
}
}
/**
* Estimates the collision object depending on current program
*
* @param program Current program running
* @param delay Time to wait in ms
* @param destructionMode If collision object is a robot KILL IT ^^
* @return collision Estimated collision object
*/
public String estimateCollision(String program, int delay) {
float[] leftSample = new float[leftSensor.sampleSize()];
float[] rightSample = new float[rightSensor.sampleSize()];
int leftTouch = 0;
int rightTouch = 0;
int touchCount = 0;
boolean leftTouched = false;
boolean rightTouched = false;
collision = "none";
int run = 0;
while (run < 4) {
leftSensor.fetchSample(leftSample, 0);
leftSensor.fetchSample(leftSample, 0);
leftTouch = (int) leftSample[0];
rightTouch = (int) rightSample[0];
LCD.drawString("Left: " + String.valueOf(leftTouch), 0, 2);
LCD.drawString("right: " + String.valueOf(rightSample), 0, 3);
// for Bridge, ChainBridge, Elevator, Rolls, Seesaw
// Elevator dürfte nicht vorkommen
// TODO: Collision mit einem Sensor Roboter
if (leftTouch != 0 && rightTouch != 0) {
drive.stop();
Delay.msDelay(delay);
touchCount++;
if (touchCount == 2) {
collision = "Wall";
break;
}
} else if (leftTouch != 0) {
leftTouched = true;
drive.stop();
Delay.msDelay(delay);
if (!rightTouched) {
touchCount++;
} else {
rightTouched = false;
}
if (touchCount == 2) {
collision = "Left Wall";
break;
}
} else if (rightTouch != 0) {
rightTouched = true;
drive.stop();
Delay.msDelay(delay);
if (!leftTouched) {
touchCount++;
} else {
leftTouched = false;
}
if (touchCount == 2) {
collision = "Right Wall";
break;
}
} else if (touchCount < 2 && touchCount != 0) {
touchCount = 0;
collision = "Robot";
break;
}
run++;
}
if ((program == "ChainBridge"
|| program == "LineFollowing"
|| program == "Rolls"
|| program == "Seesaw"
|| program == "Maze")
&& collision == "Robot") {
if (destructionMode) {
killThemAll("");
} else {
drive.stop();
Delay.msDelay(TIME_TO_WAIT);
}
} else if (program == "Elevator") {
elevatorCollision();
} else if (program == "Bridge") {
} else if (program == "EndBoss") {
}
return collision;
}