Simon Malih / Mbed 2 deprecated WareHouse

Warehouse Rover main program

Dependencies:   mbed Team2_Library

WareHouse.cpp@1:e4b822ee4fdf, 2022-03-11 (annotated)

Committer:
simon9987
Date:
Fri Mar 11 04:13:50 2022 +0000
Revision:
1:e4b822ee4fdf
N

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon9987 1:e4b822ee4fdf 1 #include "Common.h"
simon9987 1:e4b822ee4fdf 2
simon9987 1:e4b822ee4fdf 3 bool enteredReverse = false;
simon9987 1:e4b822ee4fdf 4
simon9987 1:e4b822ee4fdf 5 bool obstacleAvoidance(){
simon9987 1:e4b822ee4fdf 6 getDistance();
simon9987 1:e4b822ee4fdf 7 const unsigned int frontRight = sensorDistance[0], frontLeft = sensorDistance[1], rear = sensorDistance[2];
simon9987 1:e4b822ee4fdf 8
simon9987 1:e4b822ee4fdf 9 if((avoidObstacle(frontRight) && avoidObstacle(frontLeft)) || enteredReverse){ //all sensors are blocked car needs to stop and terminate break out of movement
simon9987 1:e4b822ee4fdf 10 if(avoidObstacle(rear)){
simon9987 1:e4b822ee4fdf 11 buzzerTerminate();
simon9987 1:e4b822ee4fdf 12 sendMessage("Terminating program - car is stuck!\r\n");
simon9987 1:e4b822ee4fdf 13 stop();
simon9987 1:e4b822ee4fdf 14 resetAll();
simon9987 1:e4b822ee4fdf 15 return true;
simon9987 1:e4b822ee4fdf 16 }
simon9987 1:e4b822ee4fdf 17 moveReverse();
simon9987 1:e4b822ee4fdf 18 enteredReverse = true;
simon9987 1:e4b822ee4fdf 19 } else if(avoidObstacle(frontRight)){ //turn right if obstacle detected on right sonar sensor
simon9987 1:e4b822ee4fdf 20 steerLeft();
simon9987 1:e4b822ee4fdf 21 moveForward();
simon9987 1:e4b822ee4fdf 22
simon9987 1:e4b822ee4fdf 23 } else if(avoidObstacle(frontLeft)){ //turn left if obstacle detected on left sonar sensor
simon9987 1:e4b822ee4fdf 24 steerRight();
simon9987 1:e4b822ee4fdf 25 moveForward();
simon9987 1:e4b822ee4fdf 26 } else {
simon9987 1:e4b822ee4fdf 27 resetSteering(); //straighten steering wheel
simon9987 1:e4b822ee4fdf 28 //if previously reversing continue to reverse else move forward
simon9987 1:e4b822ee4fdf 29 if(enteredReverse)
simon9987 1:e4b822ee4fdf 30 moveReverse();
simon9987 1:e4b822ee4fdf 31 else
simon9987 1:e4b822ee4fdf 32 moveForward();
simon9987 1:e4b822ee4fdf 33 }
simon9987 1:e4b822ee4fdf 34 return false;
simon9987 1:e4b822ee4fdf 35 }
simon9987 1:e4b822ee4fdf 36
simon9987 1:e4b822ee4fdf 37 bool checkColour(){
simon9987 1:e4b822ee4fdf 38 const int waitTime = 2;
simon9987 1:e4b822ee4fdf 39
simon9987 1:e4b822ee4fdf 40 Colour RED = Colour(255,99,71);
simon9987 1:e4b822ee4fdf 41 Colour BLUE = Colour(0,10,255);
simon9987 1:e4b822ee4fdf 42 Colour YELLOW = Colour(255,255,0);
simon9987 1:e4b822ee4fdf 43 Colour GREEN = Colour(0, 128, 0);
simon9987 1:e4b822ee4fdf 44 Colour BLACK = Colour(0, 0, 0);
simon9987 1:e4b822ee4fdf 45
simon9987 1:e4b822ee4fdf 46 Colour currentColour = getColour();
simon9987 1:e4b822ee4fdf 47 currentColour.setColourTolerance(8,8,8); // set room for error for slightly varying versions of the colour
simon9987 1:e4b822ee4fdf 48
simon9987 1:e4b822ee4fdf 49 //notify operator to collect package and wait 60 seconds
simon9987 1:e4b822ee4fdf 50 if(currentColour.equals(BLACK)){
simon9987 1:e4b822ee4fdf 51 stop();
simon9987 1:e4b822ee4fdf 52 sendMessage("Car has arrived. Please collect package\r\n");
simon9987 1:e4b822ee4fdf 53 for(int i = 0; i<30; ++i)
simon9987 1:e4b822ee4fdf 54 buzzerPackage(); //beeps for 60 seconds to also notify operator
simon9987 1:e4b822ee4fdf 55 resetAll();
simon9987 1:e4b822ee4fdf 56 return true;
simon9987 1:e4b822ee4fdf 57 }
simon9987 1:e4b822ee4fdf 58
simon9987 1:e4b822ee4fdf 59 if(currentColour.equals(YELLOW)){
simon9987 1:e4b822ee4fdf 60 steerRight(); //turn right if yellow is detected
simon9987 1:e4b822ee4fdf 61 wait(waitTime);
simon9987 1:e4b822ee4fdf 62 } else if(currentColour.equals(RED)){
simon9987 1:e4b822ee4fdf 63 steerLeft(); //turn left if red is detected
simon9987 1:e4b822ee4fdf 64 wait(waitTime);
simon9987 1:e4b822ee4fdf 65 } else if(currentColour.equals(BLUE) && packageDestination == 1){
simon9987 1:e4b822ee4fdf 66 steerLeft(); // if blue is detected and the package destination is set to one
simon9987 1:e4b822ee4fdf 67 wait(waitTime);
simon9987 1:e4b822ee4fdf 68 } else if(currentColour.equals(GREEN) && packageDestination == 2){
simon9987 1:e4b822ee4fdf 69 steerLeft(); //turn left if green is detected and the package destination is set to two
simon9987 1:e4b822ee4fdf 70 wait(waitTime);
simon9987 1:e4b822ee4fdf 71 } else
simon9987 1:e4b822ee4fdf 72 resetSteering();
simon9987 1:e4b822ee4fdf 73
simon9987 1:e4b822ee4fdf 74 moveForward();
simon9987 1:e4b822ee4fdf 75 return false;
simon9987 1:e4b822ee4fdf 76 }
simon9987 1:e4b822ee4fdf 77
simon9987 1:e4b822ee4fdf 78 int main() {
simon9987 1:e4b822ee4fdf 79 setUpMovement(0.2, -0.2, -0.7);
simon9987 1:e4b822ee4fdf 80 setUpRGB();
simon9987 1:e4b822ee4fdf 81 packageDestination = getPackageDestination(); //returns int which correlates to package destination
simon9987 1:e4b822ee4fdf 82
simon9987 1:e4b822ee4fdf 83 while (true){
simon9987 1:e4b822ee4fdf 84 if(checkColour()) // break loop if car has reached package destination
simon9987 1:e4b822ee4fdf 85 break;
simon9987 1:e4b822ee4fdf 86
simon9987 1:e4b822ee4fdf 87 if(obstacleAvoidance()){ //break loop if car is unable to move
simon9987 1:e4b822ee4fdf 88 break;
simon9987 1:e4b822ee4fdf 89 }
simon9987 1:e4b822ee4fdf 90 }
simon9987 1:e4b822ee4fdf 91
simon9987 1:e4b822ee4fdf 92 sendMessage("Rc car has terminated!\r\n"); // notify operator that car has finished its operation
simon9987 1:e4b822ee4fdf 93 }