/**
* 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);
}
