Diff: main.cpp

Revision:

11:aaab65e884be

Parent:

10:c7e0c94c8cd1

Child:

12:0b40dc152fe2

--- a/main.cpp	Mon Mar 18 17:54:48 2019 +0000
+++ b/main.cpp	Tue Mar 19 10:39:54 2019 +0000
@@ -71,77 +71,6 @@
 
 const int sensor_addr = 41 << 1; //this should just be equal to 82, haven't touched it though
 
-
-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;
-    solenoid = OFF;
-}
-
-void SolenoidController::on()
-{
-    state = ON;
-    solenoid = ON;
-}
-
-void SolenoidController::initialize()
-{
-    paper_detected = false;
-    off();
-}
-
-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:
@@ -260,13 +189,13 @@
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
     
-    //if(lastTurn[3] = LEFT and lastTurn[2] ==LEFT and lastTurn[1] == LEFT and lastTurn[0] == LEFT) {     
-      //  wait(0.25);
+    //if(lastTurn[3] == LEFT and lastTurn[2] ==LEFT and lastTurn[1] == LEFT and lastTurn[0] == LEFT) {     
+     //   turnLeftHard();
     //}
     
     //else {
-    wait(0.05);
-    stopMotors();
+    //wait(0.1);
+    //stopMotors();
     //}
     
 
@@ -285,14 +214,8 @@
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
     
-    //if(lastTurn[3] = LEFT and lastTurn[2] ==LEFT and lastTurn[1] == LEFT and lastTurn[0] == LEFT) {     
-      //  wait(0.25);
-    //}
-    
-    //else {
-    wait(0.20);
-    stopMotors();
-    //}
+    //wait(0.25);
+    //stopMotors();
     
 
 }
@@ -310,14 +233,14 @@
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-    //if(lastTurn[3] ==RIGHT and lastTurn[2] ==RIGHT and lastTurn[1] == RIGHT and lastTurn[0] == RIGHT) {     
-      //  wait(0.25);
-        //}
+   // if(lastTurn[3] ==RIGHT and lastTurn[2] ==RIGHT and lastTurn[1] == RIGHT and lastTurn[0] == RIGHT) {     
+     //   turnRightHard();
+       // }
     
-    //else {
-    wait(0.05);
-    stopMotors();
-    //}
+   // else {
+    //wait(0.1);
+    //stopMotors();
+   // }
 }
 
 void MotorController::turnRightHard()
@@ -328,19 +251,17 @@
     lastTurn[1] = lastTurn[0];
     lastTurn[0] = RIGHT;
     
+    
     setLeftMotorMode(FORWARD);
     setRightMotorMode(BACKWARD);
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-    //if(lastTurn[3] ==RIGHT and lastTurn[2] ==RIGHT and lastTurn[1] == RIGHT and lastTurn[0] == RIGHT) {     
-      //  wait(0.25);
-        //}
+    
     
-    //else {
-    wait(0.20);
-    stopMotors();
-    //}
+    //wait(0.25);
+    //stopMotors();
+    
 }
 
 void MotorController::changeDirection(int direction)
@@ -356,36 +277,114 @@
 
         case FORWARD:
             goForward();
-            setSpeed(PWM_PERIOD_US * 0.4);
+            setSpeed(PWM_PERIOD_US * 0.45);
             break;
 
         case BACKWARD:
             goBackward();
-            setSpeed(PWM_PERIOD_US * 0.4);
+            setSpeed(PWM_PERIOD_US * 0.45);
             break;
 
         case LEFT:
             turnLeft();
-            setSpeed(PWM_PERIOD_US * 0.4);
+            setSpeed(PWM_PERIOD_US * 0.7);
             break;
 
         case RIGHT:
             turnRight();
-            setSpeed(PWM_PERIOD_US *0.4); 
+            setSpeed(PWM_PERIOD_US *0.7); 
             break;
             
         case LEFTHARD:
             turnLeftHard();
-            setSpeed(PWM_PERIOD_US*0.4);
+            setSpeed(PWM_PERIOD_US*0.85);
             break;
             
         case RIGHTHARD:
             turnRightHard();
-            setSpeed(PWM_PERIOD_US*0.4);
+            setSpeed(PWM_PERIOD_US*0.85);
             break;
     }
 }
 
+class SolenoidController
+{
+public:
+    bool state;
+
+
+    void initialize();
+    void off();
+    void on();
+    void controlLogic(bool, bool, bool, bool, MotorController *);
+
+private:
+    bool paper_detected;
+};
+
+
+void SolenoidController::off()
+{
+    state = OFF;
+    solenoid = OFF;
+}
+
+void SolenoidController::on()
+{
+    state = ON;
+    solenoid = ON;
+}
+
+void SolenoidController::initialize()
+{
+    paper_detected = false;
+    off();
+}
+
+void SolenoidController::controlLogic(bool red_path, bool blue_path, bool red_detected, bool blue_detected, MotorController *motorController)
+{
+//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();
+        motorController->goForward();
+        motorController->setSpeed(PWM_PERIOD_US * 0.45);
+        wait(2);
+    }
+
+    //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();
+        motorController->goForward();
+        motorController->setSpeed(PWM_PERIOD_US * 0.45);
+        wait(2);
+    }
+
+    //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 ColourSensor
 {
 public:
@@ -568,11 +567,11 @@
 
     if(direction == CHOOSEPATH) {
 
-        if(red_path) {
+        if(blue_path) {
             direction = LEFTHARD;
         }
 
-        else if(blue_path) {
+        else if(red_path) {
             direction = RIGHTHARD;
         } else {
             direction = FORWARD;
@@ -584,7 +583,7 @@
     return direction;
 }
 
-void bluetoothControl(MotorController motorController, SolenoidController solenoidController)
+void bluetoothControl(MotorController *motorController, SolenoidController *solenoidController)
 {
     bluetooth.baud(9600);
 
@@ -603,14 +602,14 @@
                 if(state != 'F') {
                     state = 'F';
                     speed = PWM_PERIOD_US * 0.4;
-                    motorController.setSpeed(speed);
-                    motorController.goForward();
+                    motorController->setSpeed(speed);
+                    motorController->goForward();
                 }
 
                 else {
                     speed += PWM_PERIOD_US * 0.1;
-                    motorController.setSpeed(speed);
-                    motorController.goForward();
+                    motorController->setSpeed(speed);
+                    motorController->goForward();
                 }
                 break;
 
@@ -618,14 +617,14 @@
                 if(state != 'B') {
                     state = 'B';
                     speed = PWM_PERIOD_US * 0.4;
-                    motorController.setSpeed(speed);
-                    motorController.goBackward();
+                    motorController->setSpeed(speed);
+                    motorController->goBackward();
                 }
 
                 else {
                     speed += PWM_PERIOD_US * 0.1;
-                    motorController.setSpeed(speed);
-                    motorController.goBackward();
+                    motorController->setSpeed(speed);
+                    motorController->goBackward();
                 }
                 break;
 
@@ -633,14 +632,14 @@
                 if(state != 'L') {
                     state = 'L';
                     speed = PWM_PERIOD_US * 0.4;;
-                    motorController.setSpeed(speed);
-                    motorController.turnLeft();
+                    motorController->setSpeed(speed);
+                    motorController->turnLeft();
                 }
 
                 else {
                     speed += PWM_PERIOD_US * 0.1;
-                    motorController.setSpeed(speed);
-                    motorController.turnLeft();
+                    motorController->setSpeed(speed);
+                    motorController->turnLeft();
                 }
                 break;
 
@@ -648,29 +647,29 @@
                 if(state != 'R') {
                     state = 'R';
                     speed = PWM_PERIOD_US * 0.4;
-                    motorController.setSpeed(speed);
-                    motorController.turnRight();
+                    motorController->setSpeed(speed);
+                    motorController->turnRight();
                 }
 
                 else {
                     speed += PWM_PERIOD_US * 0.1;
-                    motorController.setSpeed(speed);
-                    motorController.turnRight();
+                    motorController->setSpeed(speed);
+                    motorController->turnRight();
                 }
                 break;
 
             case 'X':
                 state = 'X';
                 speed = 0;
-                motorController.setSpeed(speed);
-                motorController.stopMotors();
+                motorController->setSpeed(speed);
+                motorController->stopMotors();
                 break;
 
             case 'S':
-                if(solenoidController.state) {
-                    solenoidController.off();
-                } else if(!solenoidController.state) {
-                    solenoidController.on();
+                if(solenoidController->state) {
+                    solenoidController->off();
+                } else if(!solenoidController->state) {
+                    solenoidController->on();
                 }
                 break;
 
@@ -710,8 +709,7 @@
     blueled =1;
 
     //Start off going straight
-    //motorController.setSpeed(PWM_PERIOD_US * 0.4);
-    //motorController.goForward();
+ 
 
 
 
@@ -722,7 +720,7 @@
 
         if(bluetooth.readable()) {
             motorController.stopMotors();
-            bluetoothControl(motorController, solenoidController);
+            bluetoothControl(&motorController, &solenoidController);
         }
 
 
@@ -744,7 +742,7 @@
             lineFollower.blue_path = true;
         }
 
-        solenoidController.controlLogic(lineFollower.red_path, lineFollower.blue_path, colourSensor.red_detected, colourSensor.blue_detected);
+        solenoidController.controlLogic(lineFollower.red_path, lineFollower.blue_path, colourSensor.red_detected, colourSensor.blue_detected, &motorController);
 
         //Blink LED every loop to ensure program isn't stuck
         //redled = !redled;