/** * This method causes the robot to travel to the absolute field location (x, y). This method * continuously call turnTo(double theta) and then set the motor speed to forward(straight). This * ensures that the heading is updated until goal is reached. */ public void travelTo(double xDestination, double yDestination) { xCurrent = odometer.getX(); yCurrent = odometer.getY(); thetaCurrent = odometer.getTheta(); double deltaTheta; double deltaX = xDestination - xCurrent; double deltaY = yDestination - yCurrent; // this is the change in angle needed to reach the destination deltaTheta = calculateDeltaTheta(deltaX, deltaY); if (Math.abs(deltaTheta) > 1) { // if theta has significant error turnBy(deltaTheta); } leftMotor.forward(); rightMotor.forward(); leftMotor.setAcceleration(1500); ; rightMotor.setAcceleration(1500); leftMotor.setSpeed(FWD_SPEED); rightMotor.setSpeed(FWD_SPEED); // totalDistance uses pythagoras theorem to calculate the total // distance needed to travel double totalDistance = Math.sqrt((deltaX) * (deltaX) + (deltaY) * (deltaY)); leftMotor.rotate(convertDistance(WHEEL_RADIUS, totalDistance), true); rightMotor.rotate(convertDistance(WHEEL_RADIUS, totalDistance), false); /* * try { Thread.sleep(1000); } catch (InterruptedException e) { } */ }
// Vertical correction (along y-axis) public void correctY() { odoY = odometer.getY(); if (generalHeading == 90) { lineNum = (int) (Math.abs((odoY - lightToCenter) / 30.48)); odometer.setY(lineNum * 30.48 + absDistance); } else if (generalHeading == 270) { lineNum = (int) (Math.abs((odoY + lightToCenter) / 30.48)); odometer.setY(lineNum * 30.48 - absDistance); } }
public String reportStatus(boolean getName) { String status; if (getName) { status = new String("x"); status += "\ty"; status += "\ttheta"; status += "\ttacho LeftDelta"; status += "\ttacho RightDelta"; status += "\tabsolute distance"; } else { status = new String("" + odometer.getX()); status += "\t" + odometer.getY(); status += "\t" + odometer.getTheta(); status += "\t" + tachoLeftDelta; status += "\t" + tachoRightDelta; status += "\t + absDistance"; status += "\t"; } return status; }
/** * 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 } } } } }