Diff: linefollower.cpp

Revision:

1:243ec35fafcd

Parent:

0:1f5782fc5ca3

Child:

2:21fb894dc9d6

--- a/linefollower.cpp	Wed May 09 15:35:31 2018 +0000
+++ b/linefollower.cpp	Wed May 16 19:12:20 2018 +0000
@@ -2,8 +2,13 @@
 #include "linefollower.h"
 #include <string>
 
+DigitalOut ledLinks(LED1);
+DigitalOut ledRechts(LED2);
+DigitalOut ledRechtdoor(LED3);
+DigitalOut ledAchteruit(LED4);
+
 namespace ProjectTwo{
-
+    
     LineFollower::LineFollower(){
         current_pos_of_line = 0.0;
         previous_pos_of_line = 0.0;
@@ -14,14 +19,11 @@
         
         m3pi.locate(0,1);
         m3pi.printf("Line PID");
-
         wait(2.0);
         m3pi.cls();
         
         CurrentLineFollowerState = STATE_INIT;
-        
-        disableStop = false;
-        foundIntersection = false;
+        vlcDecoder = Manchester();
     }
     
     void LineFollower::followTheLine(void){
@@ -34,10 +36,14 @@
             }
             case STATE_COMPUTE:
             {
+                ledLinks = 0;
+                ledRechts = 0;
+                ledRechtdoor = 0;
+                ledAchteruit = 0;
                 // Get the position of the line.
                 current_pos_of_line = m3pi.line_position();        
                 proportional = current_pos_of_line;
-        
+                
                 computeDerivative();
         
                 computeIntegral();
@@ -53,12 +59,10 @@
         
                 // limit checks
                 limitChecks();   
-                
-                checkForIntersection();  
-                
-                if(foundIntersection){
+            
+                if(checkForIntersection()){
                     CurrentLineFollowerState = STATE_STOP;
-                }else if(!foundIntersection){
+                }else if(!checkForIntersection()){
                     CurrentLineFollowerState = STATE_FORWARD;
                 }
                 
@@ -72,6 +76,17 @@
                 CurrentLineFollowerState = STATE_COMPUTE;
                 break;
             }
+            case STATE_LONG_FORWARD:
+            {
+                int i = 0;
+                do{
+                    goForwards();
+                    i ++;
+                }while(i < 750);
+                CurrentLineFollowerState = STATE_COMPUTE;
+                break;
+
+            }
             case STATE_LEFT:
             {
                 turnLeft90();
@@ -84,15 +99,37 @@
                 CurrentLineFollowerState = STATE_COMPUTE;
                 break;
             }
-            case STATE_BACKWARD:
+            case STATE_TURN180:
             {
                 CurrentLineFollowerState = STATE_COMPUTE;
+                turnRight90();
+                turnRight90();
                 break;
             }
             case STATE_STOP:
             {
                 m3pi.stop();
-                CurrentLineFollowerState = STATE_VLC;
+                m3pi.printf("K-punt");
+                
+                bool searchForLight = true;
+                int scanCounter = 0;
+                while(searchForLight){
+                    if(analogSensor.getVoltage() > 0.6){
+                        CurrentLineFollowerState = STATE_VLC;
+                        searchForLight = false;
+                    }
+                    else{
+                        Scan();
+                        scanCounter++;
+                    }
+                    
+                    if(scanCounter == 2){
+                        CurrentLineFollowerState = STATE_FORWARD;
+                        searchForLight = false;
+                    }
+                    wait(0.5);
+                }
+                
                 break;
             }
             case STATE_VLC:
@@ -101,32 +138,51 @@
                 m3pi.locate(0,0);
                 m3pi.printf("VLC");
                 m3pi.locate(0,1);
-                m3pi.printf("Activated");
-                //CurrentLineFollowerState = STATE_COMPUTE; when communication is done, decide which direction to go
-                //Call to VLC obj jelle
-                //wait(1);
-                //CurrentLineFollowerState = STATE_LEFT;
+                m3pi.printf("Activate");
+                wait(1);
+                
+                //retry decoding if detected value is equal to 5
+                do{
+                    direction_index = vlcDecoder.decode();
+                }while(direction_index == 5);
                 
-                int index = decoder.decode();
+                wait(0.5);
+                m3pi.cls();
+                m3pi.printf("DIR fnd");
+                
                 wait(1); //wait 1 sec before executing selected direction
-                switch(index){
-                    case 0:{ //links
+                switch(direction_index){
+                    case 0:{ //left
                         CurrentLineFollowerState = STATE_LEFT;
+                        ledLinks = 1;
                         break;
                     }
-                    case 1:{ //rechts
+                    case 1:{ //right
                         CurrentLineFollowerState = STATE_RIGHT;
+                        ledRechts = 1;
+                        break;
+                    }
+                    case 2:{ //forwards
+                        CurrentLineFollowerState = STATE_LONG_FORWARD;
+                        ledRechtdoor = 1;
                         break;
                     }
-                    case 2:{ //rechtdoor
-                         CurrentLineFollowerState = STATE_FORWARD;
+                    case 3:{ //backwards
+                        CurrentLineFollowerState = STATE_TURN180;
+                        ledAchteruit = 1;
                         break;
                     }
-                    case 3:{ //achteruit
-                        CurrentLineFollowerState = STATE_BACKWARD;
+                    case 4:{ //ignore
+                        CurrentLineFollowerState = STATE_LONG_FORWARD;
                         break;
                     }
+                    default:{
+                        CurrentLineFollowerState = STATE_LONG_FORWARD;
+                        m3pi.cls();
+                    }
                 }
+                
+                break;  
             }
         }
     }
@@ -188,17 +244,36 @@
         m3pi.right_motor(right);
     }
     
-    void LineFollower::checkForIntersection(){
+    void LineFollower::Scan(void){
+        for(int i = 0; i <= 2; i++){
+            m3pi.stop();
+            m3pi.right_motor(MAX_SPEED);
+            wait_ms(30);
+            m3pi.stop();
+            m3pi.right_motor(-MAX_SPEED);
+            wait_ms(20);
+            m3pi.stop();
+            m3pi.left_motor(MAX_SPEED);
+            wait_ms(20);
+            m3pi.stop();
+            m3pi.left_motor(-MAX_SPEED);
+            wait_ms(10);
+        }
+        m3pi.stop();
+    }
+    
+    bool LineFollower::checkForIntersection(){
         unsigned int values[5];
         unsigned int* valuesPtr;
         valuesPtr = values;
                 
         m3pi.get_raw_values(valuesPtr);  
-                
-        if(values[0] >= 2000 || values[4] >= 2000){
-            foundIntersection = true;
+        
+        //check 2 outermost IR sensors on both sides
+        if((values[0] > 1900 && values[1] > 1900) || (values[3] > 1900 && values[4] > 1900)){
+            return true;
         }else{
-            foundIntersection = false;
+            return false;
         }
     }