public static void main(String[] args) {
UltrasonicPoller frontPoller = new UltrasonicPoller("S4");
UltrasonicPoller sidePoller = new UltrasonicPoller("S1");
ColorSensorPoller blockPoller = new ColorSensorPoller("S2");
ColorSensorPoller groundPoller = new ColorSensorPoller("S3");
groundPoller.setMode(1);
// start the block detector thread, which will be constantly checking with the light sensor
// to see if there is a block.
boolean wifiWorked = true;
// *********************************WiFi module************************************//
// Set up WiFi connection, require data from server, parse data and disconnect from server.
WifiConnection conn = null;
Transmission t = null;
try {
conn = new WifiConnection(SERVER_IP, TEAM_NUMBER);
} catch (IOException e) {
LCD.drawString("Connection failed", 0, 1);
wifiWorked = false;
}
if (conn == null) {
LCD.drawString("Unable to find Server", 0, 5);
wifiWorked = false;
} else {
// Data received from the server is saved in "t".
// Pass the data saved in t to the relevant class
t = conn.getTransmission();
// Display the data in t
if (t == null) {
LCD.drawString("Failed to read transmission", 0, 5);
wifiWorked = false;
} else {
conn.printTransmission();
}
// Button.waitForAnyPress();
LCD.clear();
}
// ********************************WiFi module ends******************************//
if (wifiWorked) {
// variables from WIFI
int flagType = t.flagType;
StartCorner startingCorner = t.startingCorner;
int bottomLeftX = t.opponentHomeZoneBL_X;
int bottomLeftY = t.opponentHomeZoneBL_Y;
int topRightX = t.opponentHomeZoneTR_X;
int topRightY = t.opponentHomeZoneTR_Y;
int capturePointX = t.dropZone_X;
int capturePointY = t.dropZone_Y;
// variables HARDCODED
// int flagType = 1;
// StartCorner startingCorner = StartCorner.TOP_RIGHT;
// int bottomLeftX = 3;
// int bottomLeftY = 3;
// int topRightX = 5;
// int topRightY = 5;
// int capturePointX = 1;
// int capturePointY = 1;
// ***********************Initialization Module******************************//
// setup the odometer
Odometer odo = new Odometer(leftMotor, rightMotor, 30, true);
OdometerCorrector odoCorr = new OdometerCorrector(odo, groundPoller);
// setup the wall avoider
WallAvoider avoider = new WallAvoider(odo, frontPoller, sidePoller);
// set up the navigator
Navigation navi = new Navigation(odo, avoider, frontPoller, WHEEL_RADIUS, TRACK);
// set up the detector
BlockDetector blockDetector =
new BlockDetector(
blockPoller, navi, odo, frontPoller, verticalArmMotor, horizontalArmMotor, flagType);
blockDetector.start();
// set up the searcher
BlockZoneSearcher flagSearcher =
new BlockZoneSearcher(sidePoller, frontPoller, navi, odo, blockDetector);
// set up the LCD
LCDDisplay lcd = new LCDDisplay(odo, frontPoller, sidePoller, blockPoller, blockDetector);
lcd.start();
// set up the localization
LightLocalizer lsl =
new LightLocalizer(
odo, groundPoller, navi, ROBOT_CENTRE_TO_LIGHTLOCALIZATION_SENSOR, startingCorner);
USLocalizer usl =
new USLocalizer(odo, navi, frontPoller, USLocalizer.LocalizationType.FULL_CIRCLE);
// ***********************End of Initialization******************************//
double angleForSearch;
String searchDirection = "";
int searchStartX;
int searchStartY;
int firstCornerX;
int firstCornerY;
double zoneBuffer;
// we use our starting corner to determine where we want to travel to in order to search.
if (startingCorner == StartCorner.BOTTOM_LEFT || startingCorner == StartCorner.BOTTOM_RIGHT) {
searchDirection = "down";
searchStartX = bottomLeftX;
searchStartY = topRightY;
firstCornerX = bottomLeftX;
firstCornerY = bottomLeftY;
zoneBuffer = -1 * TILE_WIDTH / 3.0;
angleForSearch = 270;
} else { // it is the top left or top right
searchDirection = "up";
searchStartX = topRightX;
searchStartY = bottomLeftY;
firstCornerX = topRightX;
firstCornerY = topRightY;
zoneBuffer = TILE_WIDTH / 3.0;
angleForSearch = 90;
}
/*
* Step 1: Ultrasonic Localization
* Figure out where we are, roughly
*/
// disable the side sensor for localization so that it doesn't interfere
sidePoller.disableSensor();
// set the errors on the navigation to be large for US localization
navi.setCmError(0.5);
navi.setDegreeError(4.0);
// perform ultra-sonic localization
usl.doLocalization();
/*
* Step 2: Light Localization
* Figure out where we are, precisely
*/
// set the errors back to smaller values
navi.setCmError(0.5);
navi.setDegreeError(2.0);
// perform light-sensor localization
lsl.doLocalization();
/*
* Step 3: Travel to first corner
* Travel to the corner of the block zone in which we are going to relocalize
*/
// enable the side poller for navigating
sidePoller.enableSensor();
// speed up the robot for navigation
navi.setSlowSpeed(90);
navi.setFastSpeed(160);
// start odometry correction
odoCorr.start();
// navigation
navi.travelToAndAvoid(TILE_WIDTH * firstCornerX, TILE_WIDTH * firstCornerY);
/*
* Step 4: Relocalize
* Use the lines to relocalize (figure out where we are, again)
*/
// perform second localization
lsl.doRelocalization(TILE_WIDTH * firstCornerX, TILE_WIDTH * firstCornerY);
/*
* Step 5: Travel to second corner & rotate
* Travel to the top (or bottom) corner of the block zone, and turn to searching position
*/
// we first travel a bit away from the zone
navi.travelTo(TILE_WIDTH * firstCornerX + zoneBuffer, TILE_WIDTH * firstCornerY + zoneBuffer);
// we travel to the second corner with no avoidance (there can't be any blocks there, anyway)
navi.travelTo(TILE_WIDTH * searchStartX + zoneBuffer, TILE_WIDTH * searchStartY);
navi.turnTo(angleForSearch, true);
/*
* Step 6: Search for block
* Start to search around the perimeter for the block
*/
// lower errors
navi.setCmError(0.4);
navi.setDegreeError(3);
// find dimensions of place to search
int zoneWidth = topRightX - bottomLeftX;
int zoneHeight = topRightY - bottomLeftY;
// run searcher
flagSearcher.searchZone(searchStartX, searchStartY, zoneWidth, zoneHeight, searchDirection);
/*
* Step 7: Drive to end position
* We now have the block. Drive to the final position
*/
if (blockDetector.isFlag()) {
// we want to travel to the center of the blocks.
navi.travelToAndAvoid(
capturePointX * TILE_WIDTH + TILE_WIDTH / 2,
capturePointY * TILE_WIDTH + TILE_WIDTH / 2); // wifi
}
while (Button.waitForAnyPress() != Button.ID_ESCAPE) ;
System.exit(0);
}
}
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);
}