void Robot::turnAtIntersection(bool turnRight, unsigned long time) {
// wait for correct road on intersection to appear
Tape::update();
while (!Tape::tapePresentOnSide(turnRight) && (millis() - time) < TIME_IN_INTERSECTION) {
Tape::update();
followTape(true, turnRight);
}
// turn onto tape
Actuators::turnIntersection(turnRight);
while (!Tape::tapePresentCentreWithUpdate()) {}
}
void loop()
{
followTape();
LCD.setCursor(0,0); LCD.clear(); LCD.home() ;
LCD.print("L");
LCD.print(" ");
LCD.print(analogRead(leftTapeQRD));
LCD.print(" ");
LCD.print("R");
LCD.print(" ");
LCD.print(analogRead(rightTapeQRD));
LCD.setCursor(0,1);
LCD.print("A");
LCD.print(" ");
LCD.print(servoPosition);
LCD.print(" ");
LCD.print("E");
LCD.print(" ");
LCD.print(error);
}
bool Robot::dropOffPassenger(Direction direction, bool rightSideDropOff) {
handleIntersection(direction);
unsigned long time = millis();
Tape::resetErrors();
while( (millis() - time) < DROP_OFF_APPROACH_TIME ){
Tape::update();
followTape();
if (Collision::occuredWithUpdate()) return false;
}
Actuators::turnInPlace(TURN_FOR_PASSENGER_DROPOFF_DURATION, rightSideDropOff);
Actuators::drive(VELOCITY_SLOW, Actuators::Straight); //drive up to drop off platform
delay(DROP_OFF_PASSENGER_DRIVE_OFF_TRACK_DELAY);
Actuators::stop();
Actuators::lowerArm();
Actuators::openClaw();
delay(OPEN_FINGERS_FOR_PASSENGER_DROP_OFF_DURATION);
Actuators::drive(VELOCITY_SLOW, Actuators::Straight, true); //get back onto track
delay(DROP_OFF_PASSENGER_DRIVE_OFF_TRACK_DELAY);
Actuators::stop();
Actuators::closeClaw();
Actuators::raiseArm();
turnOntoTape(!rightSideDropOff);
return true;
}
//Main function of the robot. Drives, detects IR, and detects collisions.
//Most importantly, gives status updates to the controller so the controller
//can decide what to do.
Status Robot::cruise(Direction direction) {
handleIntersection(direction);
while (true) {
//tape follows always
Tape::update();
followTape();
//we must be at an intersection, tell this to the controller
if (Tape::atIntersection() && (millis() - lastIntersectionTime) > TIME_MIN_BETWEEN_INTERSECTIONS) {
lastIntersectionTime = millis();
Actuators::stop();
return Intersection;
}
Collision::update();
//tell the controller we collided
if (Collision::occured()) {
resetVelocity();
return Collided;
}
IR::update();
//tell the controller the status of IR signals
switch (IR::check()) {
case IR::None:
setVelocity(VELOCITY_NORMAL);
break;
case IR::WeakLeft:
if(!hasPassenger){
setVelocity(VELOCITY_SLOW);
}
break;
case IR::WeakRight:
if(!hasPassenger){
setVelocity(VELOCITY_SLOW);
}
break;
case IR::StrongLeft:
if(!hasPassenger){
Actuators::stop();
resetVelocity();
IR::resetIR();
return IRLeft;
}
break;
case IR::StrongRight:
if(!hasPassenger){
Actuators::stop();
resetVelocity();
IR::resetIR();
return IRRight;
}
break;
}
}
}
