Diff: main.cpp

Revision:

8:88e72c6deac9

Parent:

7:1e5fa5952695

Child:

9:7d74c22ed54e

--- a/main.cpp	Tue Mar 05 13:58:37 2019 +0000
+++ b/main.cpp	Thu Mar 14 14:11:58 2019 +0000
@@ -12,85 +12,143 @@
 //For line following, use the previous defines and the follwoing
 #define LEFT 3
 #define RIGHT 4
-#define LEFTHARD 5
-#define RIGHTHARD 6
+#define CHOOSEPATH 5
 
-int  count = 0;
+#define LINE_THRESHOLD 0.8
 
-DigitalOut redled(LED_RED); //Debiug LED
-DigitalOut blueled(LED_BLUE);
-//For the colour sensor
-I2C i2c(PTC9, PTC8); //pins for I2C communication (SDA, SCL)
+#define RED_CLEAR_VALUE_MAX 20000
+#define BLUE_CLEAR_VALUE_MAX 55000
+
+//Debug LED
+DigitalOut redled(LED_RED);
+
+//Connections for the Adafruit TCS34725 colour sensor
+I2C i2c(PTC9, PTC8); //(SDA, SCL)
 
 
 //Set PWMs for controlling the H-bridge for the motor speed
 PwmOut PWMmotorLeft(PTA4); //Connect to EN1 of L298N
-PwmOut PWMmotorRight(PTA5); //Connect to EN1 of L298N
+PwmOut PWMmotorRight(PTA5); //Connect to EN2 of L298N
 
-BusOut leftMotorMode(PTC17,PTC16); //Connect D4 to IN1, D5 to IN2 of L298N
-BusOut rightMotorMode(PTC13,PTC12); //Connect D6 to IN3, D7 to IN4 of L298N
+BusOut leftMotorMode(PTC17,PTC16); //Connect to IN1 and IN2 of L298N
+BusOut rightMotorMode(PTC13,PTC12); //Connect to IN3 and IN4 of L298N
 
-DigitalOut solenoid(PTC3); //Switch for the solenoid
+DigitalOut solenoid(PTC3); //For the gate of the solenoid control MOSFET
 
 //For black line detection
 AnalogIn QTR3A_1(PTB0);
 AnalogIn QTR3A_2(PTB1);
 AnalogIn QTR3A_3(PTB2);
+AnalogIn QTR3A_4(PTB3);
+AnalogIn QTR3A_5(PTC2);
+AnalogIn QTR3A_6(PTC1);
 
+//Remote control novel feature
 Serial bluetooth(PTE0,PTE1);
 
-bool red_path = false;
-bool blue_path = false;
-
-const int sensor_addr = 41 << 1;
+const int sensor_addr = 41 << 1; //this should just be equal to 82, haven't touched it though
 
 
-class SolenoidController {
-    public:
+class SolenoidController
+{
+public:
     bool state;
-    
+
+
+    void initialize();
     void off();
     void on();
+    void controlLogic(bool, bool, bool, bool);
+
+private:
+    bool paper_detected;
 };
 
-void SolenoidController::off() {
-    state = OFF; 
+
+void SolenoidController::off()
+{
+    state = OFF;
     solenoid = OFF;
 }
 
-void SolenoidController::on() {
+void SolenoidController::on()
+{
     state = ON;
     solenoid = ON;
 }
 
+void SolenoidController::initialize()
+{
+    paper_detected = false;
+    off();
+}
 
-class MotorController {
-    public:
+void SolenoidController::controlLogic(bool red_path, bool blue_path, bool red_detected, bool blue_detected)
+{
+//Logic for the solenoid based on colour detected
+
+    //Detect the first sheet of paper if blue and pick up the disc
+    if(blue_detected && !paper_detected && !state) {
+        paper_detected = true;
+        blue_path = true;
+        on();
+    }
+
+    //Detect the first sheet of paper if red and pick up the disc
+    else if(red_detected && !paper_detected && !state) {
+        paper_detected = true;
+        red_path = true;
+        on();
+    }
+
+    //Detect the end of the first sheet of paper
+    else if(!blue_detected && !red_detected && paper_detected) {
+        paper_detected = false;
+    }
+
+    //Drop the disc once the second blue paper is detected
+    else if(blue_detected && blue_path && !paper_detected && state) {
+        paper_detected = true;
+        off();
+    }
+
+    //Drop the disc once the second red paper is detected
+    else if(red_detected && red_path && !paper_detected && state) {
+        paper_detected = true;
+        off();
+    }
+}
+
+class MotorController
+{
+public:
     int state;
     int speed;
     int lastTurn;
-    
-    
+
+
     void initialize();
     void setSpeed(int pulsewidth_us);
-    void setLeftMotorSpeed(int pulsewidth_us);
-    void setRightMotorSpeed(int pulsewidth_us);
     void stopMotors();
     void goForward();
     void goBackward();
     void turnLeft();
-    void turnRight();  
+    void turnRight();
+    /*
     void turnLeftHard();
     void turnRightHard();
+    */
     void changeDirection(int direction);
-    
-    private:
+
+private:
     void setLeftMotorMode(int mode);
     void setRightMotorMode(int mode);
+    void setLeftMotorSpeed(int pulsewidth_us);
+    void setRightMotorSpeed(int pulsewidth_us);
 };
 
 void MotorController::initialize()
-{   
+{
     state = STOP;
     speed = 0;
     PWMmotorLeft.period_us(PWM_PERIOD_US);
@@ -98,6 +156,10 @@
 
 }
 
+void MotorController::setSpeed(int pulsewidth_us)
+{
+    speed = pulsewidth_us;
+}
 
 void MotorController::setLeftMotorSpeed(int pulsewidth_us)
 {
@@ -131,61 +193,49 @@
 void MotorController::goForward()
 {
     state = FORWARD;
-    
+
     setLeftMotorMode(FORWARD);
     setRightMotorMode(FORWARD);
-    
+
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-    
-    wait(0.2);
-    stopMotors();
+
+    //wait(0.2);
+    //stopMotors();
 
 }
 
 void MotorController::goBackward()
 {
     state =  BACKWARD;
-    
+
     setLeftMotorMode(BACKWARD);
     setRightMotorMode(BACKWARD);
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
 
-    wait(0.2);
-    stopMotors();
+    //wait(0.2);
+    //stopMotors();
 }
 
 void MotorController::turnLeft()
-{   
+{
     state = LEFT;
     lastTurn = LEFT;
-    
+
     setLeftMotorMode(BACKWARD);
     setRightMotorMode(FORWARD);
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
 
-    wait(0.05);
-    stopMotors();
+    /* wait(0.05);
+     stopMotors();
+     */
 }
 
-void MotorController::turnLeftHard()
-{   
-    state = LEFTHARD;
-    lastTurn = LEFTHARD;
-    setLeftMotorMode(BACKWARD);
-    setRightMotorMode(FORWARD);
-
-    setLeftMotorSpeed(speed);
-    setRightMotorSpeed(speed);
-
-    wait(0.2);
-    stopMotors();
-}
 
 
 void MotorController::turnRight()
@@ -197,486 +247,363 @@
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-    
-    
-    wait(0.05);
-    stopMotors();
-    
 }
 
-void MotorController::turnRightHard()
+void MotorController::changeDirection(int direction)
 {
-    state = RIGHTHARD;
-    lastTurn= RIGHTHARD;
-    setLeftMotorMode(FORWARD);
-    setRightMotorMode(BACKWARD);
 
-    setLeftMotorSpeed(speed);
-    setRightMotorSpeed(speed);
-    
-    
-    wait(0.2);
-    stopMotors();
-}
-    
+    switch(direction) {
 
-void MotorController::changeDirection(int direction) {
-    
-    switch(direction) {
-        
         case STOP:
             stopMotors();
             break;
-         
+
         case FORWARD:
             goForward();
             setSpeed(PWM_PERIOD_US * 0.5);
-            break;   
-        
+            break;
+
         case BACKWARD:
             goBackward();
             setSpeed(PWM_PERIOD_US * 0.5);
             break;
-        
+
         case LEFT:
             turnLeft();
             setSpeed(PWM_PERIOD_US * 0.7);
             break;
-            
+
         case RIGHT:
             turnRight();
             setSpeed(PWM_PERIOD_US *0.7);
-            break;  
-            
-        case LEFTHARD:    
-            turnLeftHard();
-            setSpeed(PWM_PERIOD_US * 0.8);
-             break;
-            
-        case RIGHTHARD:
-            turnRightHard();
-            setSpeed(PWM_PERIOD_US *0.8);
-            break;  
+            break;
     }
-    
 }
 
-void MotorController::setSpeed(int pulsewidth_us) {
-    speed = pulsewidth_us;   
-}
+class ColourSensor
+{
+public:
 
-class ColourSensor {
-    public:
-    
     bool blue_detected;
     bool red_detected;
-    
+
     void initialize();
     void read();
-    
-    private:
-    const static int RED_CLEAR_VALUE_MAX = 20000;
-    const static int BLUE_CLEAR_VALUE_MAX = 55000; 
-    
 };
 
 
-void ColourSensor::initialize() {
-    
+void ColourSensor::initialize()
+{
+
     i2c.frequency(200000);
-    
+
     blue_detected = false;
     red_detected = false;
-    
+
     char id_regval[1] = {146};
     char data[1] = {0};
     i2c.write(sensor_addr,id_regval,1, true);
     i2c.read(sensor_addr,data,1,false);
-    
-    /*if (data[0]==68) {
-        myled = 0;
-        wait (2); 
-        myled = 1;
-        } else {
-        myled = 1; 
-    }*/
-    
+
     char timing_register[2] = {129,0};
     i2c.write(sensor_addr,timing_register,2,false);
-    
+
     char control_register[2] = {143,0};
     i2c.write(sensor_addr,control_register,2,false);
-    
+
     char enable_register[2] = {128,3};
     i2c.write(sensor_addr,enable_register,2,false);
 }
 
-void ColourSensor::read() {
-    
-        char clear_reg[1] = {148};
-        char clear_data[2] = {0,0};
-        i2c.write(sensor_addr,clear_reg,1, true);
-        i2c.read(sensor_addr,clear_data,2, false);
-        
-        int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
-        
-        char red_reg[1] = {150};
-        char red_data[2] = {0,0};
-        i2c.write(sensor_addr,red_reg,1, true);
-        i2c.read(sensor_addr,red_data,2, false);
-        
-        int red_value = ((int)red_data[1] << 8) | red_data[0];
-        
-        char green_reg[1] = {152};
-        char green_data[2] = {0,0};
-        i2c.write(sensor_addr,green_reg,1, true);
-        i2c.read(sensor_addr,green_data,2, false);
-        
-        int green_value = ((int)green_data[1] << 8) | green_data[0];
-        
-        char blue_reg[1] = {154};
-        char blue_data[2] = {0,0};
-        i2c.write(sensor_addr,blue_reg,1, true);
-        i2c.read(sensor_addr,blue_data,2, false);
-        
-        int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
-        
-        
-        //Detect the colour of the paper
-        
-        if(clear_value < RED_CLEAR_VALUE_MAX && (red_value > (blue_value*2))) {
-            red_detected = true;
-        }
-        
-        else {
-            red_detected = false;
-        }
-        
-        if(clear_value < BLUE_CLEAR_VALUE_MAX && blue_value > (red_value*2)) {
-            blue_detected = true;
-        }
-        
-        else {
-            blue_detected = false;   
-        }
-    
-    
-}
+void ColourSensor::read()
+{
+
+    char clear_reg[1] = {148};
+    char clear_data[2] = {0,0};
+    i2c.write(sensor_addr,clear_reg,1, true);
+    i2c.read(sensor_addr,clear_data,2, false);
+
+    int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
+
+    char red_reg[1] = {150};
+    char red_data[2] = {0,0};
+    i2c.write(sensor_addr,red_reg,1, true);
+    i2c.read(sensor_addr,red_data,2, false);
+
+    int red_value = ((int)red_data[1] << 8) | red_data[0];
+
+    char green_reg[1] = {152};
+    char green_data[2] = {0,0};
+    i2c.write(sensor_addr,green_reg,1, true);
+    i2c.read(sensor_addr,green_data,2, false);
+
+    int green_value = ((int)green_data[1] << 8) | green_data[0];
 
-class LineFollower {
-    
-    public:
-    bool lineDetected1;
-    bool lineDetected2;
-    bool lineDetected3;
-    int direction;
-    
-    void initialize();
-    void readSensors();
-    int chooseDirection(int);
-    
-    private:
-    
-    const static float LINE_THRESHOLD = 0.8;
-    void readSensor1();
-    void readSensor2();
-    void readSensor3();
-    
-    
-        
-};
+    char blue_reg[1] = {154};
+    char blue_data[2] = {0,0};
+    i2c.write(sensor_addr,blue_reg,1, true);
+    i2c.read(sensor_addr,blue_data,2, false);
 
-void LineFollower::initialize() {
-    lineDetected1 = false;
-    lineDetected2 = false;
-    lineDetected3 = false;
-    direction = STOP;
-}
+    int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
+
+
+    //Detect the colour of the paper
 
-void LineFollower::readSensor1() {
-    if(QTR3A_1.read() > LINE_THRESHOLD) {
-        lineDetected1 = true;
+    //Red is detected if their is the unfiltered light is below a threshold
+    //and there is at least twice as much red light copmared to blue light
+    if(clear_value < RED_CLEAR_VALUE_MAX && (red_value > (blue_value*2))) {
+        red_detected = true;
     }
-    
-    else {
-        lineDetected1 = false;
-        }
-}
 
-void LineFollower::readSensor2() {
-    if(QTR3A_2.read() > LINE_THRESHOLD) {
-        lineDetected2 = true;
-    }
-    
     else {
-        lineDetected2 = false;
-        } 
-}
-
-void LineFollower::readSensor3() {
-    if(QTR3A_3.read() > LINE_THRESHOLD) {
-        lineDetected3 = true;
+        red_detected = false;
     }
-    
+
+
+    //Similar to detection for red, but with a different threshold
+    if(clear_value < BLUE_CLEAR_VALUE_MAX && blue_value > (red_value*2)) {
+        blue_detected = true;
+    }
+
     else {
-        lineDetected3 = false;
-        }  
-}
-
-void LineFollower::readSensors() {
-    readSensor1();
-    readSensor2();
-    readSensor3();   
+        blue_detected = false;
+    }
 }
 
-//this doesntt seem gto be working correctly
-/*int LineFollower::chooseDirection() {
-    
-    int sensorData = 0x00 & ((((int) lineDetected1) << 2) + (((int) lineDetected2) << 1) + ((int) lineDetected3));
-    sensorData = sensorData & 0x07;
-    
-    switch(sensorData) {
-        
-        //000
-        case 0x0:
-            direction = STOP;
-            break;
-        
-        //001
-        case 0x1:
-            direction = RIGHT;
-            break;
-        
-        //010
-        case 0x2:
-            direction = FORWARD;
-            break;
-        
-        //011
-        case 0x3:
-            direction = RIGHT;
-            break;
-        
-        //100
-        case 0x4:
-            direction = LEFT;
-            break;
-        
-        //101
-        case 0x5:
-            if(red_path) {
-                direction = LEFT;
-            }
-            
-            if(blue_path) {
-                direction = RIGHT;
-            }
-            
-            break;
-            
-        //110    
-        case 0x06:
-            direction = RIGHT;
-            break;
-            
-        //111
-        case 0x7:
-            direction = FORWARD;
-            break;
-        
-        default:
-            direction = FORWARD;
-            break;
-     }       
-    return direction;
-}*/
+class LineFollower
+{
+
+public:
+    bool lineDetected[6];
+    int direction;
+    bool red_path;
+    bool blue_path;
+
+    void initialize();
+    void readSensors();
+    int chooseDirection(const char*);
+};
+
+void LineFollower::initialize()
+{
+
+    for(int i = 0; i < 6; i++) {
+        lineDetected[i] = false;
+    }
+
+    direction = STOP;
+    red_path = false;
+    blue_path = false;
+}
+
+void LineFollower::readSensors()
+{
+    if(QTR3A_1.read() > LINE_THRESHOLD) {
+        lineDetected[0] = true;
+    }
+
+    else {
+        lineDetected[0] = false;
+    }
 
-int LineFollower::chooseDirection(int lastTurn) {
-    
-        
-        //000
-        if(!lineDetected1 && !lineDetected2 && !lineDetected3) {
+    if(QTR3A_2.read() > LINE_THRESHOLD) {
+        lineDetected[1] = true;
+    }
+
+    else {
+        lineDetected[1] = false;
+    }
+    if(QTR3A_3.read() > LINE_THRESHOLD) {
+        lineDetected[2] = true;
+    }
+
+    else {
+        lineDetected[2] = false;
+    }
+    if(QTR3A_4.read() > LINE_THRESHOLD) {
+        lineDetected[3] = true;
+    }
 
-            if (count>3 and lastTurn==RIGHT) {
-                direction = RIGHTHARD;
-                count = 0;
-                }
-            else if(count>3 and lastTurn==LEFT) {
-                direction = LEFTHARD;
-                count =0;
-                }
-            
-            else {
-                direction = lastTurn;
-                }
-                
-            count++;
-        }
-        
-        //001
-        else if(!lineDetected1 && !lineDetected2 && lineDetected3) {
-            direction = RIGHT;
-        }
-        
-        //010
-        else if(!lineDetected1 && lineDetected2 && !lineDetected3) {
-            direction = FORWARD;
-        }
-        
-        //011
-        else if(!lineDetected1 && lineDetected2 && lineDetected3) {
-            direction = RIGHT;
-        }
-        
-        //100
-        else if(lineDetected1 && !lineDetected2 && !lineDetected3) {
-            direction = LEFT;
-        }
-        
-        //101
-        else if(lineDetected1 && !lineDetected2 && lineDetected3) {
-            if(red_path) {
-                direction = LEFT;
-            }
-            
-            if(blue_path) {
-                direction = RIGHT;
-            }
-            
-        }
-            
-        //110    
-        else if(lineDetected1 && lineDetected2 && !lineDetected3) {
-            direction = LEFT;
-        }
-            
-        //111
-        else if(lineDetected1 && lineDetected2 && lineDetected3) {
-            direction = FORWARD;
-        }
-        
-        else {
-            direction = FORWARD;
-     }       
-    return direction;
+    else {
+        lineDetected[3] = false;
+    }
+    if(QTR3A_5.read() > LINE_THRESHOLD) {
+        lineDetected[4] = true;
+    }
+
+    else {
+        lineDetected[4] = false;
+    }
+    if(QTR3A_6.read() > LINE_THRESHOLD) {
+        lineDetected[5] = true;
+    }
+
+    else {
+        lineDetected[5] = false;
+    }
 }
 
 
+int LineFollower::chooseDirection(const char lookupTable[])
+{
 
-void bluetoothControl(MotorController motorController) {
-     bluetooth.baud(9600);
+    int direction = STOP;
+
+    char sensorData = 0x3F | ((lineDetected[5] << 5) + (lineDetected[4] << 4) + (lineDetected[3]<< 3) +
+                              (lineDetected[2] << 2) + (lineDetected[1] << 1) + (lineDetected[0]));
+
+    direction = lookupTable[sensorData];
+
+    if(direction == CHOOSEPATH) {
+
+        if(red_path) {
+            direction = LEFT;
+        }
+
+        else if(blue_path) {
+            direction = RIGHT;
+        } else {
+            direction = FORWARD;
+
+        }
+    }
     
+     return direction;
+}
+void bluetoothControl(MotorController motorController, SolenoidController solenoidController)
+{
+    bluetooth.baud(9600);
+
     char c = '0';
     char state = 'F';
     int speed = 0;
 
-    while(true) {
-        
-        c='0';
-        
-        if(bluetooth.readable()) {
-            c = bluetooth.getc();
-        }
-        
-        
-        
+    while(bluetooth.readable()) {
+
+        c = bluetooth.getc();
+
         switch(c) {
-            
+
             case 'F':
                 if(state != 'F') {
                     state = 'F';
-                    speed = 400;
+                    speed = PWM_PERIOD_US * 0.4;
+                    motorController.setSpeed(speed);
+                    motorController.goForward();
+                }
+
+                else {
+                    speed += PWM_PERIOD_US * 0.1;
                     motorController.setSpeed(speed);
                     motorController.goForward();
                 }
-                
-                else {
-                    speed += 100;
-                    motorController.setSpeed(speed);
-                    motorController.goForward();
-                    }
                 break;
-                
+
             case 'B':
                 if(state != 'B') {
                     state = 'B';
-                    speed = 400;
+                    speed = PWM_PERIOD_US * 0.4;
+                    motorController.setSpeed(speed);
+                    motorController.goBackward();
+                }
+
+                else {
+                    speed += PWM_PERIOD_US * 0.1;
                     motorController.setSpeed(speed);
                     motorController.goBackward();
                 }
-                
-                else {
-                    speed += 100;
-                    motorController.setSpeed(speed);
-                    motorController.goBackward();
-                    }
                 break;
-                
-             case 'L':
+
+            case 'L':
                 if(state != 'L') {
                     state = 'L';
-                    speed = 800;
+                    speed = PWM_PERIOD_US * 0.4;;
+                    motorController.setSpeed(speed);
+                    motorController.turnLeft();
+                }
+
+                else {
+                    speed += PWM_PERIOD_US * 0.1;
                     motorController.setSpeed(speed);
                     motorController.turnLeft();
                 }
-                
-                else {
-                    speed += 100;
-                    motorController.setSpeed(speed);
-                    motorController.turnLeft();
-                    }
-                break;       
-             
-             case 'R':
+                break;
+
+            case 'R':
                 if(state != 'R') {
                     state = 'R';
-                    speed = 800;
+                    speed = PWM_PERIOD_US * 0.4;
+                    motorController.setSpeed(speed);
+                    motorController.turnRight();
+                }
+
+                else {
+                    speed += PWM_PERIOD_US * 0.1;
                     motorController.setSpeed(speed);
                     motorController.turnRight();
                 }
-                
-                else {
-                    speed += 100;
-                    motorController.setSpeed(speed);
-                    motorController.turnRight();
-                    }
                 break;
-                
-            case 'S':
-                state = 'S';
+
+            case 'X':
+                state = 'X';
                 speed = 0;
                 motorController.setSpeed(speed);
                 motorController.stopMotors();
-                break;     
-                        
+                break;
+
+            case 'S':
+                if(solenoidController.state) {
+                    solenoidController.off();
+                } else if(!solenoidController.state) {
+                    solenoidController.on();
+                }
+                break;
+
         }
+
+        c='0';
+
     }
 }
 
 
 
-int main() {
-    
-    //Blink LED to let you know it's on
-    redled = 0;
-    wait(0.5);
-    redled = 1;
-    wait(0.5);
-    redled = 0;
+
+int main()
+{
 
-    bool paper_detected = false;
-    
+    //A lookup table of which direction to turn the robot based on the values of all 6 sensor readings
+    //e.g. 0 = 000000 meaning no sensors detect a line and directionLookup[0] = STOP
+    //e.g. 12 = 001100 meaning the middle two sensors detect a line directionLookup[12] = FORWARD
+    const static char directionLookup[64] = {STOP, RIGHT, RIGHT, RIGHT, RIGHT, RIGHT, RIGHT, RIGHT, RIGHT, LEFT, //0-9
+                                             CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, FORWARD, LEFT, RIGHT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, //10-19
+                                             CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, RIGHT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, //20-29
+                                             FORWARD, FORWARD, LEFT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, //30-39
+                                             RIGHT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, //40-49
+                                             CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, //50-59
+                                             LEFT, CHOOSEPATH, FORWARD, FORWARD
+                                            }; //60-63
+
+    bool path_found = false;
+
     MotorController motorController;
     SolenoidController solenoidController;
     LineFollower lineFollower;
     ColourSensor colourSensor;
-    
+
+
     motorController.initialize();
     lineFollower.initialize();
     colourSensor.initialize();
-    solenoidController.off();
-    
-    
+    solenoidController.initialize();
+
+    //Blink LED after reset
+    redled = 1;
+    wait(0.5);
+    redled = 0;
+    wait(1);
+    redled = 1;
+
     //Start off going straight
     motorController.setSpeed(PWM_PERIOD_US * 0.4);
     motorController.goForward();
@@ -684,53 +611,32 @@
 
 
     while(true) {
-        redled = !redled;
-        
+
+
         if(bluetooth.readable()) {
-            bluetoothControl(motorController);
-            }
-        
-    
-        
-        lineFollower.readSensors();
-        motorController.changeDirection(lineFollower.chooseDirection(motorController.lastTurn));
-        
-        colourSensor.read();
-        
-        //Logic for the solenoid based on colour detected
-            
-        //Detect the first sheet of paper if blue and pick up the disc
-        if(colourSensor.blue_detected && !paper_detected && !solenoidController.state) {
-            paper_detected = true;
-            blue_path = true;
-            solenoidController.on(); 
+            bluetoothControl(motorController, solenoidController);
         }
-            
-            //Detect the first sheet of paper if red and pick up the disc
-        if(colourSensor.red_detected && !paper_detected && !solenoidController.state) {
-            paper_detected = true;
-            red_path = true;
-            solenoidController.on(); 
-        }
-             
-        //Detect the end of the first sheet of paper   
-        if(!colourSensor.blue_detected && !colourSensor.red_detected && paper_detected) {
-            paper_detected = false;
+
+
+
+        lineFollower.readSensors();
+        motorController.changeDirection(lineFollower.chooseDirection(directionLookup));
+
+
+        if(colourSensor.red_detected and !path_found) {
+            path_found = true;
+            lineFollower.red_path = true;
         }
-            
-        //Drop the disc once the second blue paper is detected
-        if(colourSensor.blue_detected && blue_path && !paper_detected && solenoidController.state) {
-            paper_detected = true;
-            solenoidController.off();
+
+        else if(colourSensor.blue_detected and !path_found) {
+            path_found = true;
+            lineFollower.blue_path = true;
         }
-        
-        //Drop the disc once the second red paper is detected   
-        if(colourSensor.red_detected && red_path && !paper_detected && solenoidController.state) {
-            paper_detected = true;
-            solenoidController.off();
-        }
-        
-        blueled = !blueled;
-    }              
-        
+
+        solenoidController.controlLogic(lineFollower.red_path, lineFollower.blue_path, colourSensor.red_detected, colourSensor.blue_detected);
+
+        //Blink LED every loop to ensure program isn't stuck
+        redled = !redled;
+    }
+
 }