Diff: WareHouse.cpp

Revision:

1:e4b822ee4fdf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WareHouse.cpp	Fri Mar 11 04:13:50 2022 +0000
@@ -0,0 +1,93 @@
+#include "Common.h"
+
+bool enteredReverse = false;
+
+bool obstacleAvoidance(){
+    getDistance();
+    const unsigned int frontRight = sensorDistance[0], frontLeft = sensorDistance[1], rear = sensorDistance[2];
+
+    if((avoidObstacle(frontRight) && avoidObstacle(frontLeft)) || enteredReverse){ //all sensors are blocked car needs to stop and terminate break out of movement
+        if(avoidObstacle(rear)){
+            buzzerTerminate();
+            sendMessage("Terminating program - car is stuck!\r\n");
+            stop();
+            resetAll();
+            return true;
+        }
+        moveReverse();
+        enteredReverse = true;
+    } else if(avoidObstacle(frontRight)){ //turn right if obstacle detected on right sonar sensor
+        steerLeft();
+        moveForward();
+
+    } else if(avoidObstacle(frontLeft)){ //turn left if obstacle detected on left sonar sensor
+        steerRight();
+        moveForward();
+    } else {
+        resetSteering(); //straighten steering wheel
+        //if previously reversing continue to reverse else move forward
+        if(enteredReverse)
+            moveReverse();
+        else
+            moveForward();
+    }
+    return false;
+}
+
+bool checkColour(){
+    const int waitTime = 2;
+
+    Colour RED = Colour(255,99,71);
+    Colour BLUE = Colour(0,10,255);
+    Colour YELLOW = Colour(255,255,0);
+    Colour GREEN = Colour(0, 128, 0);
+    Colour BLACK = Colour(0, 0, 0);
+
+    Colour currentColour = getColour();
+    currentColour.setColourTolerance(8,8,8); // set room for error for slightly varying versions of the colour
+
+    //notify operator to collect package and wait 60 seconds
+    if(currentColour.equals(BLACK)){
+        stop();
+        sendMessage("Car has arrived. Please collect package\r\n");
+        for(int i = 0; i<30; ++i)
+            buzzerPackage(); //beeps for 60 seconds to also notify operator
+        resetAll();
+        return true;
+    }
+
+    if(currentColour.equals(YELLOW)){
+        steerRight(); //turn right if yellow is detected
+        wait(waitTime);
+    } else if(currentColour.equals(RED)){
+        steerLeft(); //turn left if red is detected
+        wait(waitTime);
+    } else if(currentColour.equals(BLUE) && packageDestination == 1){
+        steerLeft(); // if blue is detected and the package destination is set to one
+        wait(waitTime);
+    } else if(currentColour.equals(GREEN) && packageDestination == 2){
+        steerLeft(); //turn left if green is detected and the package destination is set to two
+        wait(waitTime);
+    } else
+        resetSteering();
+
+    moveForward();
+    return false;
+}
+
+int main() {
+    setUpMovement(0.2, -0.2, -0.7);
+    setUpRGB();
+    packageDestination = getPackageDestination(); //returns int which correlates to package destination
+
+    while (true){
+        if(checkColour()) // break loop if car has reached package destination
+            break;
+
+        if(obstacleAvoidance()){ //break loop if car is unable to move
+            break;
+        }
+    }
+
+    sendMessage("Rc car has terminated!\r\n"); // notify operator that car has finished its operation
+}