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); }
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); } }
public double getWallUsValue() { return 0.40 * (1 + (0.181818 * Math.abs(Math.sin(Math.toRadians(2 * odo.getAng() + 180))))); }