Diff: main.cpp

Revision:

10:c7e0c94c8cd1

Parent:

9:7d74c22ed54e

Child:

11:aaab65e884be

--- a/main.cpp	Fri Mar 15 11:52:32 2019 +0000
+++ b/main.cpp	Mon Mar 18 17:54:48 2019 +0000
@@ -1,4 +1,4 @@
-#include "mbed.h"
+ #include "mbed.h"
 
 
 Serial pc(USBTX, USBRX);
@@ -15,14 +15,18 @@
 #define LEFT 3
 #define RIGHT 4
 #define CHOOSEPATH 5
+#define LOST 6
+#define LEFTHARD 7
+#define RIGHTHARD 8
 
 #define LINE_THRESHOLD 0.8
 
-#define RED_CLEAR_VALUE_MAX 20000
-#define BLUE_CLEAR_VALUE_MAX 55000
+#define RED_CLEAR_VALUE_MAX 44000
+#define BLUE_CLEAR_VALUE_MAX 66000
 
 //Debug LED
 DigitalOut redled(LED_RED);
+DigitalOut blueled(LED_BLUE);
 
 //Connections for the Adafruit TCS34725 colour sensor
 I2C i2c(PTC9, PTC8); //(SDA, SCL)
@@ -47,23 +51,23 @@
 
 
 //Remote control novel feature
-Serial bluetooth(PTE0,PTE1);
+Serial bluetooth(PTE0,PTE1); //TX, RX 
 
 
 //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
 
-int directionLookup[64] = {STOP, RIGHT, RIGHT, RIGHT, FORWARD, RIGHT, FORWARD, RIGHT, FORWARD, LEFT, //0-9
-                            CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, FORWARD, FORWARD, RIGHT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, //10-19
-                            CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, FORWARD, 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
-                            FORWARD, CHOOSEPATH, FORWARD, FORWARD
-                           }; //60-63
+int directionLookup[64] = {LOST, RIGHTHARD, RIGHT, RIGHT, FORWARD, RIGHT, RIGHT, RIGHT, FORWARD, FORWARD, //0-9
+                           FORWARD, RIGHT, FORWARD, FORWARD, FORWARD, FORWARD, RIGHT, CHOOSEPATH, CHOOSEPATH, RIGHT, //10-19
+                           FORWARD, CHOOSEPATH, RIGHT, RIGHT, LEFT, CHOOSEPATH, LEFT, CHOOSEPATH, FORWARD, FORWARD, //20-29
+                           FORWARD, FORWARD, LEFTHARD, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, RIGHT, CHOOSEPATH, //30-39
+                           LEFT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, FORWARD, RIGHT, LEFT, CHOOSEPATH, //40-49
+                           LEFT, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, LEFT, CHOOSEPATH, //50-59
+                           FORWARD, FORWARD, FORWARD, FORWARD
+                          }; //60-63
 
-                           
+
 
 const int sensor_addr = 41 << 1; //this should just be equal to 82, haven't touched it though
 
@@ -120,7 +124,7 @@
         on();
     }
 
-     //Detect the end of the first sheet of paper
+    //Detect the end of the first sheet of paper
     else if(!blue_detected && !red_detected && paper_detected) {
         paper_detected = false;
     }
@@ -143,7 +147,7 @@
 public:
     int state;
     int speed;
-    int lastTurn;
+    int lastTurn[4];
 
 
     void initialize();
@@ -153,11 +157,14 @@
     void goBackward();
     void turnLeft();
     void turnRight();
+    void turnLeftHard();
+    void turnRightHard();
     /*
     void turnLeftHard();
     void turnRightHard();
     */
     void changeDirection(int);
+    
 
 private:
     void setLeftMotorMode(int);
@@ -242,35 +249,105 @@
 void MotorController::turnLeft()
 {
     state = LEFT;
-    lastTurn = LEFT;
+    lastTurn[3] = lastTurn[2];
+    lastTurn[2] = lastTurn[1];
+    lastTurn[1] = lastTurn[0];
+    lastTurn[0] = LEFT;
+
+    setLeftMotorMode(BACKWARD);
+    setRightMotorMode(FORWARD);
+
+    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.05);
+    stopMotors();
+    //}
+    
+
+}
+void MotorController::turnLeftHard()
+{
+    state = LEFT;
+    lastTurn[3] = lastTurn[2];
+    lastTurn[2] = lastTurn[1];
+    lastTurn[1] = lastTurn[0];
+    lastTurn[0] = LEFT;
 
     setLeftMotorMode(BACKWARD);
     setRightMotorMode(FORWARD);
 
     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.05);
-     stopMotors();
-     */
 }
 
-
-
 void MotorController::turnRight()
 {
     state = RIGHT;
-    lastTurn = RIGHT;
+    lastTurn[3] = lastTurn[2];
+    lastTurn[2] = lastTurn[1];
+    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.05);
+    stopMotors();
+    //}
+}
+
+void MotorController::turnRightHard()
+{
+    state = RIGHT;
+    lastTurn[3] = lastTurn[2];
+    lastTurn[2] = lastTurn[1];
+    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();
+    //}
 }
 
 void MotorController::changeDirection(int direction)
 {
-
+    if(direction == LOST) {
+        direction = lastTurn[0];
+    }
     switch(direction) {
 
         case STOP:
@@ -279,22 +356,32 @@
 
         case FORWARD:
             goForward();
-            setSpeed(PWM_PERIOD_US * 0.5);
+            setSpeed(PWM_PERIOD_US * 0.4);
             break;
 
         case BACKWARD:
             goBackward();
-            setSpeed(PWM_PERIOD_US * 0.5);
+            setSpeed(PWM_PERIOD_US * 0.4);
             break;
 
         case LEFT:
             turnLeft();
-            setSpeed(PWM_PERIOD_US * 0.7);
+            setSpeed(PWM_PERIOD_US * 0.4);
             break;
 
         case RIGHT:
             turnRight();
-            setSpeed(PWM_PERIOD_US *0.7);
+            setSpeed(PWM_PERIOD_US *0.4); 
+            break;
+            
+        case LEFTHARD:
+            turnLeftHard();
+            setSpeed(PWM_PERIOD_US*0.4);
+            break;
+            
+        case RIGHTHARD:
+            turnRightHard();
+            setSpeed(PWM_PERIOD_US*0.4);
             break;
     }
 }
@@ -466,36 +553,37 @@
 
 int LineFollower::chooseDirection()
 {
-    
+
 
     int direction = STOP;
 
     int sensorData = 0x3F & ((lineDetected[5] << 5) + (lineDetected[4] << 4) + (lineDetected[3]<< 3) +
-                              (lineDetected[2] << 2) + (lineDetected[1] << 1) + (lineDetected[0]));
-                              
+                             (lineDetected[2] << 2) + (lineDetected[1] << 1) + (lineDetected[0]));
+
     pc.printf("\n\rSensor data = %d", sensorData);
 
     direction = directionLookup[sensorData];
 
     pc.printf("\n\rTable result = %d", direction);
-    
+
     if(direction == CHOOSEPATH) {
 
         if(red_path) {
-            direction = LEFT;
+            direction = LEFTHARD;
         }
 
         else if(blue_path) {
-            direction = RIGHT;
+            direction = RIGHTHARD;
         } else {
             direction = FORWARD;
 
         }
     }
-    
+
     pc.printf("\n\rChosen direction = %d", direction);
     return direction;
 }
+
 void bluetoothControl(MotorController motorController, SolenoidController solenoidController)
 {
     bluetooth.baud(9600);
@@ -504,10 +592,11 @@
     char state = 'F';
     int speed = 0;
 
-    while(bluetooth.readable()) {
-
+    while(true) {
+        
+        if(bluetooth.readable()) {
         c = bluetooth.getc();
-
+        } else {continue;}
         switch(c) {
 
             case 'F':
@@ -618,10 +707,11 @@
     redled = 0;
     wait(1);
     redled = 1;
+    blueled =1;
 
     //Start off going straight
-    motorController.setSpeed(PWM_PERIOD_US * 0.4);
-    motorController.goForward();
+    //motorController.setSpeed(PWM_PERIOD_US * 0.4);
+    //motorController.goForward();
 
 
 
@@ -631,6 +721,7 @@
 
 
         if(bluetooth.readable()) {
+            motorController.stopMotors();
             bluetoothControl(motorController, solenoidController);
         }
 
@@ -639,13 +730,16 @@
         lineFollower.readSensors();
         motorController.changeDirection(lineFollower.chooseDirection());
 
+        colourSensor.read();
 
         if(colourSensor.red_detected and !path_found) {
+            redled = 0;
             path_found = true;
             lineFollower.red_path = true;
         }
 
         else if(colourSensor.blue_detected and !path_found) {
+            blueled = 0;
             path_found = true;
             lineFollower.blue_path = true;
         }
@@ -653,8 +747,8 @@
         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;
-        pc.printf("\n\rRound the loop");
+        //redled = !redled;
+        //pc.printf("\n\rRound the loop");
     }
 
 }