/**
* This recursive method is responsible for finding the blue styrofoam block and returning it to
* the green/red zone. The search and detect process will begin when the robot has reached the
* green/red zone.
*
* @param x The x position of where the robot will start the search for the blue styrofoam block
* @param y The y position of where the robot will start search for the blue styrofoam block
*/
public void doSearchAndDetect(double x, double y) { // Searching inside the green/red zone
if (firstTry) {
odox = odometer.getX();
odoy = odometer.getY();
odotheta = odometer.getTheta();
dashboard.turnLeft(40);
try {
Thread.sleep(750);
} catch (Exception e) {
}
} else if (Math.abs(odox - odometer.getX()) > 15 && Math.abs(odoy - odometer.getY()) > 15) {
odox = odometer.getX();
odoy = odometer.getY();
odotheta = odometer.getTheta();
dashboard.turnLeft(40);
try {
Thread.sleep(750);
} catch (Exception e) {
}
}
firstTry = false;
// Initializing the heading angle when a robot detects something 35 cm away
detectionAngle = odometer.getTheta();
Point greenZone = new Point((int) x, (int) y); // Remembering green zone
dashboard.turnLeft(70); // Beginning the 360 degrees scan
boolean clawIsDown = false;
// The boolean found never becomes true so the robot will keep spinning until
// it finds something then moves to a different zone or it will complete 1 full
// revolution from its starting angle then move to a different zone.
while (!found) {
// Condition for the robot to move to a different area to search since it has
// completed one full revolution
if (Math.abs(odox - odometer.getX()) < 10
&& Math.abs(odoy - odometer.getY()) < 10
&& Math.abs(odotheta - odometer.getTheta())
< 5) { // Case where no objects in search radius
if (directionCounter
== 1) { // First time around, go to a point 15 cm south west of the green zone bottom
// left corner
directionCounter++;
navigation.travelTo(greenZone.getX() - 15, greenZone.getY() - 15, false);
} else if (directionCounter
== 2) { // Second time around, go below the middle of the green zone
directionCounter++;
navigation.travelTo(greenZone.getX() + 30, greenZone.getY() - 15, false);
} else if (directionCounter == 3) { // Third time, go north easy of of the green zone
directionCounter++;
navigation.travelTo(greenZone.getX() + 30, greenZone.getY() + 45, false);
} else if (directionCounter
== 4) { // Last time around, go to the middle of the playing field
directionCounter = 1;
navigation.travelTo(180, 180, false);
}
// After traveling to the new scan zone, perform the search and scan
doSearchAndDetect(greenZone.getX(), greenZone.getY());
} else {
// If the ultrasonic sensor senses something within 35 centimeters, go to the object
if (usLow.getFilteredDistance() <= 35
&& Math.abs(odometer.getTheta() - badHeading) > 30) { // Range to go check out object
detectionAngle = odometer.getTheta(); // Saving heading angle when the object is detected
blockAngle = odometer.getTheta();
previousDistance = usLow.getFilteredDistance();
navigation.turnTo(
detectionAngle
- 30.0); // Turning to past where object was detected so it can analyze the full
// block
Stopwatch stopwatch = new Stopwatch();
// Scanning whole object for 3 seconds to find shortest point of the object to robot
while (stopwatch.elapsed() < (3 * Math.pow(10, 3))) {
dashboard.turnRight(
70); // Turning to the right to analyze object since it overturned initially
if (usLow.getFilteredDistance() < previousDistance) {
dashboard.stop();
previousDistance = usLow.getFilteredDistance();
blockAngle = odometer.getTheta();
}
}
dashboard.stop();
navigation.turnTo(blockAngle); // Turn to block
// Remembering tacho count in case object is a wooden block
// so it will back up to exactly the same spot
originalTacho = dashboard.getRightTacho();
Stopwatch watch = new Stopwatch();
while (usLow.getFilteredDistance() >= 10) {
// If robot hasn't reached object in 8 seconds, most likely nothing there and false
// positive
if (watch.elapsed() > 9 * Math.pow(10, 3)) {
badHeading = odometer.getTheta();
newTacho = dashboard.getRightTacho();
dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
dashboard.stop();
doSearchAndDetect(greenZone.getX(), greenZone.getY());
}
// Travel to the object by going forward
dashboard.goForward(200);
}
// Making a check for a wooden block by using the top ultrasonic sensor
if (usHigh.getFilteredDistance()
< 25) { // Brown Block so back up distance traveled to the block
dashboard.stop();
badHeading =
odometer
.getTheta(); // Saving the angle of the wooden block so it doesn't go back when
// it sees it again
newTacho = dashboard.getRightTacho();
dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
dashboard.stop();
doSearchAndDetect(greenZone.getX(), greenZone.getY());
} else { // Go forward a bit more to get closer to the styrofoam block
for (int i = 0; i < 200; i++) {
dashboard.goForward(70);
}
}
// 2nd check for brown block to make sure when closer to object
if (usHigh.getFilteredDistance() < 25) {
dashboard.stop();
badHeading = odometer.getTheta();
newTacho = dashboard.getRightTacho();
dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
dashboard.stop();
doSearchAndDetect(greenZone.getX(), greenZone.getY());
} else { // Return to dropzone then drop off and re-do scan
newTacho = dashboard.getRightTacho();
// If doing search but the block is in the green/red zone, don't pick it up again
if ((odometer.getX() > greenZone.getX() && odometer.getY() > greenZone.getY())
&& (odometer.getX() < greenZone.getX() + 30
&& odometer.getY() < greenZone.getY() + 30)) {
badHeading = odometer.getTheta();
dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
dashboard.stop();
doSearchAndDetect(greenZone.getX(), greenZone.getY());
}
dashboard.stop();
// Reached a good distance from the styrofoam block so back up
// and get ready for the juggle.
Stopwatch secondWatch = new Stopwatch();
while (secondWatch.elapsed() < 3 * Math.pow(10, 3)) {
dashboard.goBackward(100);
}
dashboard.stop();
if (!clawIsDown) {
// Arms are currently up so drop them since the block is
// detected and close by
lifter.dropClaw();
clawIsDown = true;
}
// Driving forward until the block is within 7 cm
while (usLow.getFilteredDistance() >= 7) {
dashboard.goForward(100);
}
juggle(); // Juggle to reposition the block so it is easier to pick
dashboard.stop();
// Drive forward 8 cm so the block will be picked up perfectly
dashboard.goForward(8, false);
// Remember where the block was picked up so the robot
// will return here after dropping off in the designated zone
previousPickUp.setLocation(odometer.getX(), odometer.getY());
dashboard.stop();
lifter.lift(); // Picking up the block
isCarrying = true;
// Driving back to the green zone (a bit further to account) for the arms
// and at this distance, the block is dropped off in the middle of the zone
navigation.travelTo(greenZone.getX() + 45, greenZone.getY() + 45, false);
// If it is the nth*2 time, robot will stack.
if (!firstIteration) {
lifter.stackLift();
}
navigation.turnTo(180); // Turning so the block will end up in the middle of the zone
dashboard.goForward(
2,
false); // Going forward 2 cm so even closer to block (used for stacking purposes)
lifter.dropBlock();
Stopwatch secondStop = new Stopwatch();
// After dropping it off, back up then lift arms
while (secondStop.elapsed() < 3.5 * Math.pow(10, 3)) {
dashboard.goBackward(100);
}
dashboard.stop();
lifter.postLift();
firstIteration = !firstIteration; // Only making stacks of 2 so reverse boolean
navigation.travelTo(
greenZone.getX() - 15,
greenZone.getY(),
false); // Travel to side of green zone so blocks are not touched when navigating
// out of the zone
navigation.travelTo(
previousPickUp.getX(),
previousPickUp.getY(),
false); // Return to previous position of where styrofoam block was found
doSearchAndDetect(
greenZone.getX(), greenZone.getY()); // Do search and detect at the new position
}
}
}
}
}
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);
}
}
