Diff: main.cpp

Revision:

13:3f239fa868f2

Parent:

12:0b40dc152fe2

--- a/main.cpp	Tue Mar 19 14:46:27 2019 +0000
+++ b/main.cpp	Wed Mar 20 13:07:42 2019 +0000
@@ -1,7 +1,5 @@
- #include "mbed.h"
+#include "mbed.h"
 
-
-Serial pc(USBTX, USBRX);
 //For the solenoid
 #define OFF 0
 #define ON 1
@@ -11,7 +9,8 @@
 #define STOP 0
 #define FORWARD 1
 #define BACKWARD 2
-//For line following, use the previous defines and the follwoing
+
+//For line following
 #define LEFT 3
 #define RIGHT 4
 #define CHOOSEPATH 5
@@ -19,23 +18,26 @@
 #define LEFTHARD 7
 #define RIGHTHARD 8
 
+//The Analog threshold for line detection in the range of 0 to 1
 #define LINE_THRESHOLD 0.8
 
+//The red and blue paper reflect different levels of clear light.
+//These maximums are needed so it doesn't think the floow is the paper
 #define RED_CLEAR_VALUE_MAX 44000
 #define BLUE_CLEAR_VALUE_MAX 66000
 
-//Debug LED
+//\Allows the user to see if the robot is taking the red or blue path
 DigitalOut redled(LED_RED);
 DigitalOut blueled(LED_BLUE);
 
 //Connections for the Adafruit TCS34725 colour sensor
 I2C i2c(PTC9, PTC8); //(SDA, SCL)
 
-
-//Set PWMs for controlling the H-bridge for the motor speed
+//Set PWMs for controlling the H-bridge for the motor speeds
 PwmOut PWMmotorLeft(PTA4); //Connect to EN1 of L298N
 PwmOut PWMmotorRight(PTA5); //Connect to EN2 of L298N
 
+//For setting forward or backward for each set of motors
 BusOut leftMotorMode(PTC17,PTC16); //Connect to IN1 and IN2 of L298N
 BusOut rightMotorMode(PTC13,PTC12); //Connect to IN3 and IN4 of L298N
 
@@ -49,9 +51,8 @@
 AnalogIn QTR3A_5(PTB1);
 AnalogIn QTR3A_6(PTB2);
 
-
 //Remote control novel feature
-Serial bluetooth(PTE0,PTE1); //TX, RX 
+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
@@ -65,7 +66,7 @@
                            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
-                          }; 
+                          };
 
 
 
@@ -89,7 +90,7 @@
     void turnLeftHard();
     void turnRightHard();
     void changeDirection(int);
-    
+
 
 private:
     void setLeftMotorMode(int);
@@ -174,17 +175,12 @@
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-    
-    //if(lastTurn[3] == LEFT and lastTurn[2] ==LEFT and lastTurn[1] == LEFT and lastTurn[0] == LEFT) {     
-     //   turnLeftHard();
-    //}
-    
-    //else {
+
     wait(0.1);
     speed = PWM_PERIOD_US*0.4;
     goForward();
-    //}
-    
+
+
 
 }
 void MotorController::turnLeftHard()
@@ -197,12 +193,11 @@
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-    
+
     wait(0.25);
     speed = PWM_PERIOD_US*0.4;
     goForward();
-    //stopMotors();
-    
+
 
 }
 
@@ -210,15 +205,14 @@
 {
     state = RIGHT;
     lastTurn = RIGHT;
-    
+
     setLeftMotorMode(FORWARD);
     setRightMotorMode(BACKWARD);
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
 
-    //Turn for a short amount of time and then go straight
-    wait(0.1); 
+    wait(0.1);
     speed = PWM_PERIOD_US*0.4;
     goForward();
 
@@ -228,15 +222,14 @@
 {
     state = RIGHT;
     lastTurn = RIGHT;
-    
-    
+
     setLeftMotorMode(FORWARD);
     setRightMotorMode(BACKWARD);
 
     setLeftMotorSpeed(speed);
     setRightMotorSpeed(speed);
-    
-    
+
+
     wait(0.25);
     speed = PWM_PERIOD_US*0.4;
     goForward();
@@ -244,9 +237,13 @@
 
 void MotorController::changeDirection(int direction)
 {
+    //Sometimes when going around corners the robot would turn, then go straight
+    //and lose the track. If thart happens just turn the last direction it
+    //turned in before
     if(direction == LOST) {
         direction = lastTurn;
     }
+
     switch(direction) {
 
         case STOP:
@@ -255,32 +252,32 @@
 
         case FORWARD:
             goForward();
-            setSpeed(PWM_PERIOD_US * 0.40);
+            setSpeed(PWM_PERIOD_US * 0.4);
             break;
 
         case BACKWARD:
             goBackward();
-            setSpeed(PWM_PERIOD_US * 0.40);
+            setSpeed(PWM_PERIOD_US * 0.4);
             break;
 
         case LEFT:
             turnLeft();
-            setSpeed(PWM_PERIOD_US * 0.70);
+            setSpeed(PWM_PERIOD_US * 0.7);
             break;
 
         case RIGHT:
             turnRight();
-            setSpeed(PWM_PERIOD_US *0.70); 
+            setSpeed(PWM_PERIOD_US *0.7);
             break;
-            
+
         case LEFTHARD:
             turnLeftHard();
-            setSpeed(PWM_PERIOD_US*0.70);
+            setSpeed(PWM_PERIOD_US*0.7);
             break;
-            
+
         case RIGHTHARD:
             turnRightHard();
-            setSpeed(PWM_PERIOD_US*0.70);
+            setSpeed(PWM_PERIOD_US*0.7);
             break;
     }
 }
@@ -362,7 +359,6 @@
 }
 
 
-
 class ColourSensor
 {
 public:
@@ -433,7 +429,7 @@
     //Detect the colour of the paper
 
     //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
+    //and there is at least twice as much red light compared to blue light
     if(clear_value < RED_CLEAR_VALUE_MAX && (red_value > (blue_value*2))) {
         red_detected = true;
     }
@@ -530,19 +526,16 @@
 
 int LineFollower::chooseDirection()
 {
-
-
+    //Default case
     int direction = STOP;
 
+    //Create a six digit binary number where each sensor changes a bit
     int sensorData = 0x3F & ((lineDetected[5] << 5) + (lineDetected[4] << 4) + (lineDetected[3]<< 3) +
                              (lineDetected[2] << 2) + (lineDetected[1] << 1) + (lineDetected[0]));
 
-    pc.printf("\n\rSensor data = %d", sensorData);
-
+    //The number is used as the index for the llopkup table
     direction = directionLookup[sensorData];
 
-    pc.printf("\n\rTable result = %d", direction);
-
     if(direction == CHOOSEPATH) {
 
         if(blue_path) {
@@ -557,7 +550,6 @@
         }
     }
 
-    pc.printf("\n\rChosen direction = %d", direction);
     return direction;
 }
 
@@ -570,10 +562,12 @@
     int speed = 0;
 
     while(true) {
-        
+
         if(bluetooth.readable()) {
-        c = bluetooth.getc();
-        } else {continue;}
+            c = bluetooth.getc();
+        } else {
+            continue;
+        }
         switch(c) {
 
             case 'F':
@@ -664,7 +658,6 @@
 int main()
 {
 
-
     bool path_found = false;
 
     MotorController motorController;
@@ -684,13 +677,7 @@
     redled = 0;
     wait(1);
     redled = 1;
-    blueled =1;
-
-    //Start off going straight
- 
-
-
-
+    blueled = 1;
 
     while(true) {
 
@@ -722,9 +709,6 @@
 
         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;
-        //pc.printf("\n\rRound the loop");
     }
 
 }