Warehouse Rover main program

Dependencies:   mbed Team2_Library

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers WareHouse.cpp Source File

WareHouse.cpp

00001 #include "Common.h"
00002 
00003 bool enteredReverse = false;
00004 
00005 bool obstacleAvoidance(){
00006     getDistance();
00007     const unsigned int frontRight = sensorDistance[0], frontLeft = sensorDistance[1], rear = sensorDistance[2];
00008 
00009     if((avoidObstacle(frontRight) && avoidObstacle(frontLeft)) || enteredReverse){ //all sensors are blocked car needs to stop and terminate break out of movement
00010         if(avoidObstacle(rear)){
00011             buzzerTerminate();
00012             sendMessage("Terminating program - car is stuck!\r\n");
00013             stop();
00014             resetAll();
00015             return true;
00016         }
00017         moveReverse();
00018         enteredReverse = true;
00019     } else if(avoidObstacle(frontRight)){ //turn right if obstacle detected on right sonar sensor
00020         steerLeft();
00021         moveForward();
00022 
00023     } else if(avoidObstacle(frontLeft)){ //turn left if obstacle detected on left sonar sensor
00024         steerRight();
00025         moveForward();
00026     } else {
00027         resetSteering(); //straighten steering wheel
00028         //if previously reversing continue to reverse else move forward
00029         if(enteredReverse)
00030             moveReverse();
00031         else
00032             moveForward();
00033     }
00034     return false;
00035 }
00036 
00037 bool checkColour(){
00038     const int waitTime = 2;
00039 
00040     Colour RED = Colour(255,99,71);
00041     Colour BLUE = Colour(0,10,255);
00042     Colour YELLOW = Colour(255,255,0);
00043     Colour GREEN = Colour(0, 128, 0);
00044     Colour BLACK = Colour(0, 0, 0);
00045 
00046     Colour currentColour = getColour();
00047     currentColour.setColourTolerance(8,8,8); // set room for error for slightly varying versions of the colour
00048 
00049     //notify operator to collect package and wait 60 seconds
00050     if(currentColour.equals(BLACK)){
00051         stop();
00052         sendMessage("Car has arrived. Please collect package\r\n");
00053         for(int i = 0; i<30; ++i)
00054             buzzerPackage(); //beeps for 60 seconds to also notify operator
00055         resetAll();
00056         return true;
00057     }
00058 
00059     if(currentColour.equals(YELLOW)){
00060         steerRight(); //turn right if yellow is detected
00061         wait(waitTime);
00062     } else if(currentColour.equals(RED)){
00063         steerLeft(); //turn left if red is detected
00064         wait(waitTime);
00065     } else if(currentColour.equals(BLUE) && packageDestination == 1){
00066         steerLeft(); // if blue is detected and the package destination is set to one
00067         wait(waitTime);
00068     } else if(currentColour.equals(GREEN) && packageDestination == 2){
00069         steerLeft(); //turn left if green is detected and the package destination is set to two
00070         wait(waitTime);
00071     } else
00072         resetSteering();
00073 
00074     moveForward();
00075     return false;
00076 }
00077 
00078 int main() {
00079     setUpMovement(0.2, -0.2, -0.7);
00080     setUpRGB();
00081     packageDestination = getPackageDestination(); //returns int which correlates to package destination
00082 
00083     while (true){
00084         if(checkColour()) // break loop if car has reached package destination
00085             break;
00086 
00087         if(obstacleAvoidance()){ //break loop if car is unable to move
00088             break;
00089         }
00090     }
00091 
00092     sendMessage("Rc car has terminated!\r\n"); // notify operator that car has finished its operation
00093 }