Diff: main.cpp

Revision:

2:f0610c06721d

Parent:

1:c5b58b10970d

Child:

3:d7bda2ab309d

--- a/main.cpp	Wed Jan 16 17:38:09 2019 +0000
+++ b/main.cpp	Wed Jan 30 18:12:53 2019 +0000
@@ -4,7 +4,6 @@
 #define OFF 0
 #define ON 1
 
-
 //For motor control
 #define PWM_PERIOD_US 1000 //For setting PWM periods to 1 milliseconds. I made this number up
 #define STOP 0
@@ -23,7 +22,7 @@
 //For the colour sensor
 I2C i2c(I2C_SDA, I2C_SCL); //pins for I2C communication (SDA, SCL)
 
-int sensor_addr = 41 << 1;
+
 
 //Set PWMs for controlling the H-bridge for the motor speed
 PwmOut PWMmotorLeft(PTA4); //Connect to EN1 of L298N
@@ -35,13 +34,21 @@
 DigitalOut solenoid(PTA3); //Switch for the solenoid
 
 //For black line detection
+AnalogIn QTR3A_1(A0);
+AnalogIn QTR3A_2(A1);
+AnalogIn QTR3A_3(A2);
 DigitalIn lineSensor1(PTA6);
 DigitalIn lineSensor2(PTA7);
 DigitalIn lineSensor3(PTA8);
 
+Serial bluetooth(PTE0,PTE1);
+
 bool red_path = false;
 bool blue_path = false;
 
+int sensor_addr = 41 << 1;
+
+
 class SolenoidController {
     public:
     bool state;
@@ -207,11 +214,17 @@
 
 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; 
+    
 };
 
 
@@ -278,7 +291,7 @@
         
         //Detect the colour of the paper
         
-        if(red_value >= COLOUR_THRESHOLD) {
+        if(clear_value < RED_CLEAR_VALUE_MAX && (red_value > (blue_value*2))) {
             red_detected = true;
         }
         
@@ -286,7 +299,7 @@
             red_detected = false;
         }
         
-        if(blue_value >= COLOUR_THRESHOLD) {
+        if(clear_value < BLUE_CLEAR_VALUE_MAX && blue_value > (red_value*2)) {
             blue_detected = true;
         }
         
@@ -298,6 +311,7 @@
 }
 
 class LineFollower {
+    
     public:
     bool lineDetected1;
     bool lineDetected2;
@@ -305,13 +319,17 @@
     int direction;
     
     void initialize();
+    void readSensors();
+    int chooseDirection();
     
+    private:
+    
+    const static float LINE_THRESHOLD = 0.5;
     void readSensor1();
     void readSensor2();
     void readSensor3();
-    void readSensors();
     
-    int chooseDirection();
+    
         
 };
 
@@ -323,15 +341,33 @@
 }
 
 void LineFollower::readSensor1() {
-    lineDetected1 = lineSensor1;   
+    if(QTR3A_1.read() > LINE_THRESHOLD) {
+        lineDetected1 = true;
+    }
+    
+    else {
+        lineDetected1 = false;
+        }
 }
 
 void LineFollower::readSensor2() {
-    lineDetected2 = lineSensor2;   
+    if(QTR3A_2.read() > LINE_THRESHOLD) {
+        lineDetected2 = true;
+    }
+    
+    else {
+        lineDetected2 = false;
+        } 
 }
 
 void LineFollower::readSensor3() {
-    lineDetected3 = lineSensor3;   
+    if(QTR3A_3.read() > LINE_THRESHOLD) {
+        lineDetected3 = true;
+    }
+    
+    else {
+        lineDetected3 = false;
+        }  
 }
 
 void LineFollower::readSensors() {
@@ -342,13 +378,13 @@
 
 int LineFollower::chooseDirection() {
     
-    int sensorData = 0x00 & ((lineDetected1 << 2) + (lineDetected2 << 1) + (lineDetected3));
-    sensorData = sensorData & 0x07
+    int sensorData = 0x00 & ((((int) lineDetected1) << 2) + (((int) lineDetected2) << 1) + ((int) lineDetected3));
+    sensorData = sensorData & 0x07;
     
     switch(sensorData) {
         
         //000
-        case 0x0
+        case 0x0:
             direction = STOP;
             break;
         
@@ -401,6 +437,99 @@
     return direction;
 }
 
+
+void bluetoothControl(MotorController motorController) {
+     bluetooth.baud(9600);
+    
+    char c = '0';
+    char state = 'F';
+    int speed = 0;
+
+    while(true) {
+        
+        c='0';
+        
+        if(bluetooth.readable()) {
+            c = bluetooth.getc();
+        }
+        
+        
+        
+        switch(c) {
+            
+            case 'F':
+                if(state != 'F') {
+                    state = 'F';
+                    speed = 400;
+                    motorController.setSpeed(speed);
+                    motorController.goForward();
+                }
+                
+                else {
+                    speed += 100;
+                    motorController.setSpeed(speed);
+                    motorController.goForward();
+                    }
+                break;
+                
+            case 'B':
+                if(state != 'B') {
+                    state = 'B';
+                    speed = 400;
+                    motorController.setSpeed(speed);
+                    motorController.goBackward();
+                }
+                
+                else {
+                    speed += 100;
+                    motorController.setSpeed(speed);
+                    motorController.goBackward();
+                    }
+                break;
+                
+             case 'L':
+                if(state != 'L') {
+                    state = 'L';
+                    speed = 800;
+                    motorController.setSpeed(speed);
+                    motorController.turnLeft();
+                }
+                
+                else {
+                    speed += 100;
+                    motorController.setSpeed(speed);
+                    motorController.turnLeft();
+                    }
+                break;       
+             
+             case 'R':
+                if(state != 'R') {
+                    state = 'R';
+                    speed = 800;
+                    motorController.setSpeed(speed);
+                    motorController.turnRight();
+                }
+                
+                else {
+                    speed += 100;
+                    motorController.setSpeed(speed);
+                    motorController.turnRight();
+                    }
+                break;
+                
+            case 'S':
+                state = 'S';
+                speed = 0;
+                motorController.setSpeed(speed);
+                motorController.stopMotors();
+                break;     
+                        
+        }
+    }
+}
+
+
+
 int main() {
     
     //Blink LED to let you know it's on
@@ -422,12 +551,18 @@
     colourSensor.initialize();
     solenoidController.off();
     
-    motorController.setSpeed(200);
+    
+    //Start off going straight
+    motorController.setSpeed(700);
     motorController.goForward();
 
 
     while(true) {
         
+        if(bluetooth.readable()) {
+            bluetoothControl(motorController);
+            }
+        
         lineFollower.readSensors();
         motorController.changeDirection(lineFollower.chooseDirection());