Warehouse Rover main program
Dependencies: mbed Team2_Library
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 }
Generated on Fri May 12 2023 02:18:07 by
1.7.2