/**
* This method "juggles" and helps position the block the robot wants to pick up. It will shift
* left and right then close its arms to straighten the block so the bottom ultrasonic sensor
* faces directly at the block.
*/
private void juggle() {
lifter.halfClose();
// Shifting left and right
dashboard.turn(-15);
dashboard.turn(30);
dashboard.turn(-15);
// Straightening the block
lifter.closeClaw();
lifter.openClaw();
}
/**
* 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
}
}
}
}
}