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; } } }