File content as of revision 1:e4b822ee4fdf:

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