void Move::avoidEdge() {
if (looker->sensesObstacle(OBST_FRONT_EDGE, MIN_DISTANCE)) {
#ifdef MOVE_DEBUG
Serial.println(P("left and right sensors detected edge"));
#endif
timedMove(MOV_BACK, 300);
rotate(120);
while (looker->sensesObstacle(OBST_FRONT_EDGE, MIN_DISTANCE)) {
stop(); // stop motors if still over cliff
}
} else if (looker->sensesObstacle(OBST_LEFT_EDGE, MIN_DISTANCE)) {
#ifdef MOVE_DEBUG
Serial.println(P("left sensor detected edge"));
#endif
timedMove(MOV_BACK, 100);
rotate(30);
} else if (looker->sensesObstacle(OBST_RIGHT_EDGE, MIN_DISTANCE)) {
#ifdef MOVE_DEBUG
Serial.println(P("right sensor detected edge"));
#endif
timedMove(MOV_BACK, 100);
rotate(-30);
}
}
int Move::moveToAvoidObstacle() {
// Need to stop and look around to see if
// there is a clear path
stop();
// You learned this when you first got your driver's
// license: look both ways at a stop sign.
int leftDistance = looker->lookAt(DIR_LEFT);
delay(500);
int rightDistance = looker->lookAt(DIR_RIGHT);
int maxDistance = max(leftDistance, rightDistance);
if (maxDistance > CLEAR_DISTANCE) {
// We got clearance. Rotate as appropriate and move on
rotate(appropriateRotationAngle(leftDistance, rightDistance));
} else if (maxDistance < (CLEAR_DISTANCE / 2)) {
// If the maximum clearance we have is less than half
// the acceptable clearance, move backwards for
// one second, then turn around and go the other direction
timedMove(MOV_BACK, 1000);
rotate(-180);
} else {
// The maximum clearance was more than half the acceptable
// clearance. Go right or left depending on which had the
// most clearance
rotate(appropriateRotationAngle(leftDistance, rightDistance));
}
}
task main()
{
initializeRobot();
waitForStart();
turnLeft(_45DEGREES - 35);
delayStop(10);
moveBackward(32);
delayStop(10);
turnLeft(_90DEGREES);
delayStop(10);
timedMove(5, 100, -1); // 5 seconds, 100 power, backwards
stopDrive();
delayStop(0);
timedMove(3, 100, 1); // 3 seconds, 100 power, forward
stopDrive();
delayStop(0);
timedMove(4, 100, -1); // 4 seconds, 100 power, backwards
stopDrive();
delayStop(0);
gotoIR_FORWARD();
storeEncoderValues();
resetEnc();
delayStop(300);
turnRight(_90DEGREES - 35);
resetEnc();
delayStop(0);
}
