Diff: main.cpp

Revision:

4:ace17b63da3c

Parent:

3:d7bda2ab309d

Child:

5:92510334cdfe

diff -r d7bda2ab309d -r ace17b63da3c main.cpp
--- a/main.cpp	Wed Jan 30 18:14:58 2019 +0000
+++ b/main.cpp	Fri Mar 01 15:22:34 2019 +0000
@@ -7,20 +7,16 @@
 //For motor control
 #define PWM_PERIOD_US 1000 //For setting PWM periods to 1 milliseconds. I made this number up
 #define STOP 0
-#define FORWARD 1
-#define BACKWARD 2
+#define FORWARD 2
+#define BACKWARD 1
 //For line following, use the previous defines and the follwoing
-#define LEFT 3
-#define RIGHT 4
+#define LEFT 4
+#define RIGHT 3
 
-//For colour detection
-#define COLOUR_THRESHOLD 150 //Will have to tune this value
-
-
-DigitalOut myled(LED1); // Debug led
-
+DigitalOut redled(LED_RED); //Debiug LED
+DigitalOut blueled(LED_BLUE);
 //For the colour sensor
-I2C i2c(PTC11, PTC10); //pins for I2C communication (SDA, SCL)
+I2C i2c(PTC9, PTC8); //pins for I2C communication (SDA, SCL)
 
 
 
@@ -31,19 +27,19 @@
 BusOut leftMotorMode(PTC17,PTC16); //Connect D4 to IN1, D5 to IN2 of L298N
 BusOut rightMotorMode(PTC13,PTC12); //Connect D6 to IN3, D7 to IN4 of L298N
 
-DigitalOut solenoid(PTA3); //Switch for the solenoid
+DigitalOut solenoid(PTC3); //Switch for the solenoid
 
 //For black line detection
-AnalogIn QTR3A_1(A0);
-AnalogIn QTR3A_2(A1);
-AnalogIn QTR3A_3(A2);
+AnalogIn QTR3A_1(PTB0);
+AnalogIn QTR3A_2(PTB1);
+AnalogIn QTR3A_3(PTB2);
 
 Serial bluetooth(PTE0,PTE1);
 
 bool red_path = false;
 bool blue_path = false;
 
-int sensor_addr = 41 << 1;
+const int sensor_addr = 41 << 1;
 
 
 class SolenoidController {
@@ -184,24 +180,23 @@
          
         case FORWARD:
             goForward();
+            setSpeed(800);
             break;   
         
         case BACKWARD:
             goBackward();
+            setSpeed(800);
             break;
         
         case LEFT:
             turnLeft();
+            setSpeed(800);
             break;
             
         case RIGHT:
             turnRight();
-            break;
-            
-        default:
-            stopMotors();
-            break;
-            
+            setSpeed(800);
+            break;  
     }
 }
 
@@ -237,13 +232,13 @@
     i2c.write(sensor_addr,id_regval,1, true);
     i2c.read(sensor_addr,data,1,false);
     
-    if (data[0]==68) {
+    /*if (data[0]==68) {
         myled = 0;
         wait (2); 
         myled = 1;
         } else {
         myled = 1; 
-    }
+    }*/
     
     char timing_register[2] = {129,0};
     i2c.write(sensor_addr,timing_register,2,false);
@@ -321,7 +316,7 @@
     
     private:
     
-    const static float LINE_THRESHOLD = 0.5;
+    const static float LINE_THRESHOLD = 0.8;
     void readSensor1();
     void readSensor2();
     void readSensor3();
@@ -373,7 +368,8 @@
     readSensor3();   
 }
 
-int LineFollower::chooseDirection() {
+//this doesntt seem gto be working correctly
+/*int LineFollower::chooseDirection() {
     
     int sensorData = 0x00 & ((((int) lineDetected1) << 2) + (((int) lineDetected2) << 1) + ((int) lineDetected3));
     sensorData = sensorData & 0x07;
@@ -432,9 +428,64 @@
             break;
      }       
     return direction;
+}*/
+
+int LineFollower::chooseDirection() {
+    
+        
+        //000
+        if(!lineDetected1 && !lineDetected2 && !lineDetected3) {
+            direction = STOP;
+        }
+        
+        //001
+        else if(!lineDetected1 && !lineDetected2 && lineDetected3) {
+            direction = RIGHT;
+        }
+        
+        //010
+        else if(!lineDetected1 && lineDetected2 && !lineDetected3) {
+            direction = FORWARD;
+        }
+        
+        //011
+        else if(!lineDetected1 && lineDetected2 && lineDetected3) {
+            direction = RIGHT;
+        }
+        
+        //100
+        else if(lineDetected1 && !lineDetected2 && !lineDetected3) {
+            direction = LEFT;
+        }
+        
+        //101
+        else if(lineDetected1 && !lineDetected2 && lineDetected3) {
+            if(red_path) {
+                direction = LEFT;
+            }
+            
+            if(blue_path) {
+                direction = RIGHT;
+            }
+            
+        }
+            
+        //110    
+        else if(lineDetected1 && lineDetected2 && !lineDetected3) {
+            direction = RIGHT;
+        }
+            
+        //111
+        else if(lineDetected1 && lineDetected2 && lineDetected3) {
+            direction = FORWARD;
+        }
+        
+        else {
+            direction = FORWARD;
+     }       
+    return direction;
 }
 
-
 void bluetoothControl(MotorController motorController) {
      bluetooth.baud(9600);
     
@@ -530,11 +581,11 @@
 int main() {
     
     //Blink LED to let you know it's on
-    myled = 0;
+    redled = 0;
     wait(0.5);
-    myled = 1;
+    redled = 1;
     wait(0.5);
-    myled = 0;
+    redled = 0;
 
     bool paper_detected = false;
     
@@ -555,6 +606,7 @@
 
 
     while(true) {
+        redled = !redled;
         
         if(bluetooth.readable()) {
             bluetoothControl(motorController);
@@ -597,6 +649,10 @@
             paper_detected = true;
             solenoidController.off();
         }
+        
+        blueled = !blueled;
+        
+        wait(0.5);
                 
     }