/** * 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) { } */ }
public void atomicVariable(DapAtomicVariable dapvar) throws DapException { DapType basetype = dapvar.getBaseType(); Odometer odom = null; if (dapvar.getRank() == 0) { // scalar odom = new ScalarOdometer(); } else { // dimensioned // get the slices from the constraint List<Slice> slices = ce.getConstrainedSlices(dapvar); // Create an odometer from the slices odom = Odometer.factory(slices, dapvar.getDimensions(), false); } while (odom.hasNext()) { Object value = values.nextValue(basetype); if (DEBUG) { System.err.printf("generate: %s = %s\n", dapvar.getFQN(), value); System.err.flush(); } try { assert (writer != null); writer.writeObject(basetype, value); } catch (IOException ioe) { throw new DapException(ioe); } odom.next(); } }
public void sequence(DapSequence seq) throws DapException { List<DapVariable> fields = seq.getFields(); Odometer odom = null; if (seq.getRank() == 0) { // scalar odom = new ScalarOdometer(); } else { // dimensioned List<Slice> slices = ce.getConstrainedSlices(seq); odom = Odometer.factory(slices, seq.getDimensions(), false); } try { while (odom.hasNext()) { // Decide how many rows for this sequence int nrows = (rowcount == 0 ? this.values.nextCount(MAXROWS) : rowcount); writer.writeObject(DapType.INT64, (long) nrows); for (int i = 0; i < nrows; i++) { for (int j = 0; j < fields.size(); j++) { DapVariable field = fields.get(j); variable(field); } } odom.next(); } } catch (IOException ioe) { throw new DapException(ioe); } }
public static void main(String[] args) { Scanner readerObject = new Scanner(System.in); String numberString = readerObject.next(); Odometer odometer = new Odometer(numberString.length()); System.out.println(); System.out.println(odometer.nextValue(new Integer(numberString))); System.out.println(odometer.previousValue(new Integer(numberString))); }
// 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); } }
// Horizontal correction (along x-axis) public void correctX() { odoX = odometer.getX(); if (generalHeading == 0) { lineNum = (int) (Math.abs((odoX - lightToCenter) / 30.48)); odometer.setX(lineNum * 30.48 + absDistance); } else if (generalHeading == 180) { lineNum = (int) (Math.abs((odoX + lightToCenter) / 30.48)); odometer.setX(lineNum * 30.48 - absDistance); } }
public void timedOut() { RConsole.println("" + odo.getAngle() + ", " + linePoller.getColourVal()); // RConsole.println("" + odo.getX() + ", " + odo.getY() + ", " + odo.getAngle()); // RConsole.println(""); // RConsole.println("" + Controller.task); // RConsole.println(""); // RConsole.println(""); // RConsole.println("X: " + (int)odo.getX()); // RConsole.println("Y: " + (int)odo.getY()); // RConsole.println("Angle: " + (int)odo.getAngle()); // RConsole.println(""); // RConsole.println(""); // RConsole.println("Search Zone: (" + Controller.ourZoneLL_X + ", " + Controller.ourZoneLL_Y + // ") to (" + Controller.ourZoneUR_X + ", " + Controller.ourZoneUR_Y + ")"); // RConsole.println("Flag coulour: " + Controller.ourFlagColour); // // // switch (Controller.task){ // case NAVIGATING: // break; // case SEARCHING: // break; // default: // break; // } }
public void Load(byte[] buffer) { ByteArrayInputStream stream = new ByteArrayInputStream(buffer); tach.Load(stream); speedo.Load(stream); odo.Load(stream); fuel.Load(stream); }
public void structure(DapStructure struct) throws DapException { List<DapVariable> fields = struct.getFields(); Odometer odom = null; if (struct.getRank() == 0) { // scalar odom = new ScalarOdometer(); } else { // dimensioned List<Slice> slices = ce.getConstrainedSlices(struct); odom = Odometer.factory(slices, struct.getDimensions(), false); } while (odom.hasNext()) { // generate a value for each field recursively for (int i = 0; i < fields.size(); i++) { DapVariable field = fields.get(i); variable(field); } odom.next(); } }
// return the general orientation of robot (0 || 90 || 180 || 270) public int getHeading() { odoTheta = odometer.getTheta(); if (odoTheta > (360 - thetaBand) && odoTheta < (0 + thetaBand)) return 0; else if (odoTheta > (90 - thetaBand) && odoTheta < (90 + thetaBand)) return 90; else if (odoTheta > (180 - thetaBand) && odoTheta < (180 - thetaBand)) return 180; else return 270; }
private static void parseLineOfSudokuBoard(SudokuFile sf, String[] values, int rowNumber) { // writes values to the row set in param rowNumber if (sf.getBoard() == null) { sf.setBoard(new int[sf.getN()][sf.getN()]); } int[][] board = sf.getBoard(); for (int i = 0; i < values.length; i++) { board[rowNumber][i] = Odometer.odometerToInt(values[i]); } }
public void timedOut() { odo.getPosition(pos); LCD.clear(); LCD.drawString("X: ", 0, 0); LCD.drawString("Y: ", 0, 1); LCD.drawString("H: ", 0, 2); LCD.drawInt((int) (pos[0]), 3, 0); LCD.drawInt((int) (pos[1]), 3, 1); LCD.drawInt((int) pos[2], 3, 2); }
// Heading Correction!! public void correctHeading() { tachoLeftDelta = Math.abs(tachoLeft2 - tachoLeft1); tachoRightDelta = Math.abs(tachoRight2 - tachoRight1); distanceErrorL = getDistanceFromTacho(tachoLeftDelta, leftRadius); distanceErrorR = getDistanceFromTacho(tachoRightDelta, rightRadius); distanceError = (distanceErrorL + distanceErrorR) / 2; thetaError = Math.toDegrees(Math.atan((distanceError / lightLToLightR))); timeDelta = time1 - time2; odometer.setTheta(getHeading() + (timeDelta / Math.abs(timeDelta)) * thetaError); }
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; }
public void doBoardSearch() { this.nav.travelTo(30, 30); this.nav.turnTo(2, false); nav.setSpeeds(-1 * ROTATION_SPEED, ROTATION_SPEED); while (!objectSeen()) {} nav.setSpeeds(0, 0); counter++; Double rightAngle = odo.getAng(); nav.setSpeeds(-1 * ROTATION_SPEED, ROTATION_SPEED); while (objectSeen()) {} nav.setSpeeds(0, 0); Double leftAngle = odo.getAng(); this.nav.turnTo(odo.getAng() - 5 - (0.5 * this.getAngleDistance(leftAngle, rightAngle)), false); this.nav.setSpeeds(0, 0); while (usPoller.getDistance() > 0.065) { this.nav.setSpeeds(50, 50); } this.nav.setSpeeds(0, 0); // go find another block this.nav.travelTo(30, 30); this.nav.turnTo(leftAngle + 20, false); nav.setSpeeds(-1 * ROTATION_SPEED, ROTATION_SPEED); while (!objectSeen()) {} nav.setSpeeds(0, 0); counter++; rightAngle = odo.getAng(); nav.setSpeeds(-1 * ROTATION_SPEED, ROTATION_SPEED); while (objectSeen()) {} nav.setSpeeds(0, 0); leftAngle = odo.getAng(); this.nav.turnTo(odo.getAng() - 5 - (0.5 * this.getAngleDistance(leftAngle, rightAngle)), false); this.nav.setSpeeds(0, 0); while (usPoller.getDistance() > 0.065) { this.nav.setSpeeds(50, 50); } this.nav.setSpeeds(0, 0); }
/** * 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 void run() { leftMotor.setAcceleration(2000); rightMotor.setAcceleration(2000); distMid = getFilteredData(); sensorMotor.rotate(45); Delay.msDelay(500); distLeft = getFilteredData(); sensorMotor.rotate(-90); Delay.msDelay(500); distRight = getFilteredData(); sensorMotor.rotate(45); if (distLeft > distMid && distLeft > distRight) { select = 0; } else if (distRight >= distMid && distRight >= distLeft) { select = 2; // turn right } if (select == 0) { leftMotor.setSpeed(ROTATE_SPEED); rightMotor.setSpeed(ROTATE_SPEED); sensorMotor.setAcceleration(1000); angleSave = odometer.getAng(); leftMotor.rotate(-convertAngle(2.2, 15, 89.8), true); rightMotor.rotate(convertAngle(2.2, 15, 89.8), false); sensorMotor.rotate(-85); while (safe == false || collide() == false) { if (getFilteredData() > 100 && filterControl < FILTER_OUT) { // bad value, do not set the distance var, however do increment the filter value filterControl++; } else if (getFilteredData() > 100) { // set distance to this.distance distance = 100; } else { // distance went below 100, therefore reset everything. filterControl = 0; distance = getFilteredData(); } perror = distance - bandCenter; amperror = perror * proportion; if (distance > 60) { // if distance >80, the robot is about to do a U-turn leftMotor.setSpeed(maxSpeed); rightMotor.setSpeed(motorStraight); // right wheel will speed up in order to turn left leftMotor.forward(); rightMotor.forward(); } /* else if(distance<8){ // if distance<8, the robot is going to hit the wall leftMotor.setSpeed(motorStraight+50); rightMotor.setSpeed(motorStraight+50); leftMotor.forward(); rightMotor.backward(); // right motor rolls backwards to respond. } */ else { // general proportion error correction rightMotor.setSpeed(motorStraight); if ((motorStraight + amperror) > maxSpeed) { leftMotor.setSpeed(maxSpeed); } // left wheel remains constant speed else { leftMotor.setSpeed((int) (motorStraight + amperror)); } // right wheel will continuously correcting its speed according to errors. leftMotor.forward(); rightMotor.forward(); } if (CloseTo(rightDegreeHelper(angleSave, odometer.getAng()), -90)) { sensorMotor.rotate(85); safe = true; } } } if (select == 2) { leftMotor.setSpeed(ROTATE_SPEED); rightMotor.setSpeed(ROTATE_SPEED); sensorMotor.setAcceleration(1000); angleSave = odometer.getAng(); leftMotor.rotate(convertAngle(2.2, 15, 89.8), true); rightMotor.rotate(-convertAngle(2.2, 15, 89.8), false); sensorMotor.rotate(85); while (safe == false || collide() == false) { if (getFilteredData() > 100 && filterControl < FILTER_OUT) { // bad value, do not set the distance var, however do increment the filter value filterControl++; } else if (getFilteredData() > 100) { // set distance to this.distance distance = 100; } else { // distance went below 100, therefore reset everything. filterControl = 0; distance = getFilteredData(); } perror = distance - bandCenter; amperror = perror * proportion; if (distance > 60) { // if distance >80, the robot is about to do a U-turn leftMotor.setSpeed(motorStraight); rightMotor.setSpeed(maxSpeed); // right wheel will speed up in order to turn left leftMotor.forward(); rightMotor.forward(); } /* else if(distance<10){ // if distance<8, the robot is going to hit the wall rightMotor.setSpeed(motorStraight+50); rightMotor.setSpeed(motorStraight+50); rightMotor.forward(); leftMotor.backward(); // right motor rolls backwards to respond. } */ else { // general proportion error correction leftMotor.setSpeed(motorStraight); if ((motorStraight + amperror) > maxSpeed) { rightMotor.setSpeed(maxSpeed); } // left wheel remains constant speed else { rightMotor.setSpeed((int) (motorStraight + amperror)); } // right wheel will continuously correcting its speed according to errors. leftMotor.forward(); rightMotor.forward(); } if (CloseTo(leftDegreeHelper(angleSave, odometer.getAng()), 90)) { sensorMotor.rotate(-85); safe = true; } } } if (collide() == true) { leftMotor.stop(true); rightMotor.stop(false); leftMotor.rotate(convertDistance(2.2, -10), true); rightMotor.rotate(convertDistance(2.2, -10), false); } }
/** * Projection X Iterator This basically returns an odometer that will iterate over the appropriate * values. * * @param var over whose dimensions to iterate * @throws DapException */ public Odometer projectionIterator(DapVariable var) throws DapException { Segment seg = findSegment(var); if (seg == null) return null; return Odometer.factory(seg.slices, seg.dimset, false); }
public double getWallUsValue() { return 0.40 * (1 + (0.181818 * Math.abs(Math.sin(Math.toRadians(2 * odo.getAng() + 180))))); }
/** * Localizes the robot using the ultrasonic sensor. Clocks the angle at which the sensor detects * each wall and calculates 0 degrees */ public void doLocalization() { angleA = 0; angleB = 0; int UCdistance, expmtDistance; expmtDistance = 38; int count = 0; boolean facingToWall = true; // rotate the robot until it sees no wall nav.rotate(165, -165); facingToWall = true; while (facingToWall) { UCdistance = getFilteredData(); if (UCdistance > expmtDistance) { count++; } if (UCdistance > expmtDistance && count >= 3) { facingToWall = false; count = 0; } } sleep(3000); // keep rotating until the robot sees a wall, then latch the angle while (!facingToWall) { UCdistance = getFilteredData(); if (UCdistance < expmtDistance) { count++; } if (UCdistance < expmtDistance && count >= 3) { facingToWall = true; angleA = odo.getTheta(); Sound.beep(); nav.stopMotor(); count = 0; } } // switch direction and wait until it sees no wall nav.rotate(-165, 165); while (facingToWall) { UCdistance = getFilteredData(); if (UCdistance > expmtDistance) { count++; } if (UCdistance > expmtDistance && count >= 3) { facingToWall = false; count = 0; } } // keep rotating until the robot sees a wall, then latch the angle sleep(3000); while (!facingToWall) { UCdistance = getFilteredData(); if (UCdistance < expmtDistance) { count++; } if (UCdistance < expmtDistance && count >= 3) { facingToWall = true; angleB = odo.getTheta(); Sound.beep(); nav.stopMotor(); } } // angleA is clockwise from angleB, so assume the average of the // angles to the right of angleB is 45 degrees past 'north' // we slightly correct the angle '45' to '44' based on the error we measured angle = 45 - (angleA - angleB) / 2; // update the odometer position (example to follow:) odo.setTheta(angle); nav.turnTo(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); }