Diff: main.cpp

Revision:

9:7d74c22ed54e

Parent:

8:88e72c6deac9

Child:

10:c7e0c94c8cd1

--- a/main.cpp	Thu Mar 14 14:11:58 2019 +0000
+++ b/main.cpp	Fri Mar 15 11:52:32 2019 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 
+
+Serial pc(USBTX, USBRX);
 //For the solenoid
 #define OFF 0
 #define ON 1
@@ -36,16 +38,33 @@
 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);
+AnalogIn QTR3A_1(PTB3);
+AnalogIn QTR3A_2(PTC2);
+AnalogIn QTR3A_3(PTC1);
+AnalogIn QTR3A_4(PTB0);
+AnalogIn QTR3A_5(PTB1);
+AnalogIn QTR3A_6(PTB2);
+
 
 //Remote control novel feature
 Serial bluetooth(PTE0,PTE1);
 
+
+//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
+
+                           
+
 const int sensor_addr = 41 << 1; //this should just be equal to 82, haven't touched it though
 
 
@@ -101,7 +120,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;
     }
@@ -138,13 +157,13 @@
     void turnLeftHard();
     void turnRightHard();
     */
-    void changeDirection(int direction);
+    void changeDirection(int);
 
 private:
-    void setLeftMotorMode(int mode);
-    void setRightMotorMode(int mode);
-    void setLeftMotorSpeed(int pulsewidth_us);
-    void setRightMotorSpeed(int pulsewidth_us);
+    void setLeftMotorMode(int);
+    void setRightMotorMode(int);
+    void setLeftMotorSpeed(int);
+    void setRightMotorSpeed(int);
 };
 
 void MotorController::initialize()
@@ -375,13 +394,13 @@
 
 public:
     bool lineDetected[6];
-    int direction;
     bool red_path;
     bool blue_path;
 
+
     void initialize();
     void readSensors();
-    int chooseDirection(const char*);
+    int chooseDirection();
 };
 
 void LineFollower::initialize()
@@ -391,9 +410,10 @@
         lineDetected[i] = false;
     }
 
-    direction = STOP;
     red_path = false;
     blue_path = false;
+
+
 }
 
 void LineFollower::readSensors()
@@ -444,16 +464,21 @@
 }
 
 
-int LineFollower::chooseDirection(const char lookupTable[])
+int LineFollower::chooseDirection()
 {
+    
 
     int direction = STOP;
 
-    char sensorData = 0x3F | ((lineDetected[5] << 5) + (lineDetected[4] << 4) + (lineDetected[3]<< 3) +
+    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);
 
-    direction = lookupTable[sensorData];
+    direction = directionLookup[sensorData];
 
+    pc.printf("\n\rTable result = %d", direction);
+    
     if(direction == CHOOSEPATH) {
 
         if(red_path) {
@@ -468,7 +493,8 @@
         }
     }
     
-     return direction;
+    pc.printf("\n\rChosen direction = %d", direction);
+    return direction;
 }
 void bluetoothControl(MotorController motorController, SolenoidController solenoidController)
 {
@@ -572,17 +598,6 @@
 int main()
 {
 
-    //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;
 
@@ -610,9 +625,11 @@
 
 
 
+
     while(true) {
 
 
+
         if(bluetooth.readable()) {
             bluetoothControl(motorController, solenoidController);
         }
@@ -620,7 +637,7 @@
 
 
         lineFollower.readSensors();
-        motorController.changeDirection(lineFollower.chooseDirection(directionLookup));
+        motorController.changeDirection(lineFollower.chooseDirection());
 
 
         if(colourSensor.red_detected and !path_found) {
@@ -637,6 +654,7 @@
 
         //Blink LED every loop to ensure program isn't stuck
         redled = !redled;
+        pc.printf("\n\rRound the loop");
     }
 
 }