Diff: linefollower.cpp

Revision:

0:1f5782fc5ca3

Child:

1:243ec35fafcd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/linefollower.cpp	Wed May 09 15:35:31 2018 +0000
@@ -0,0 +1,205 @@
+#include "mbed.h"
+#include "linefollower.h"
+#include <string>
+
+namespace ProjectTwo{
+
+    LineFollower::LineFollower(){
+        current_pos_of_line = 0.0;
+        previous_pos_of_line = 0.0;
+        derivative = 0;
+        proportional = 0;
+        integral = 0;
+        speed = MAX_SPEED;
+        
+        m3pi.locate(0,1);
+        m3pi.printf("Line PID");
+
+        wait(2.0);
+        m3pi.cls();
+        
+        CurrentLineFollowerState = STATE_INIT;
+        
+        disableStop = false;
+        foundIntersection = false;
+    }
+    
+    void LineFollower::followTheLine(void){
+        switch(CurrentLineFollowerState){              
+            case STATE_INIT:
+            {
+                m3pi.sensor_auto_calibrate();
+                CurrentLineFollowerState = STATE_COMPUTE;
+                break;
+            }
+            case STATE_COMPUTE:
+            {
+                // Get the position of the line.
+                current_pos_of_line = m3pi.line_position();        
+                proportional = current_pos_of_line;
+        
+                computeDerivative();
+        
+                computeIntegral();
+        
+                // Remember the last position.
+                previous_pos_of_line = current_pos_of_line;
+        
+                // Compute the power
+                computePower();
+        
+                // Compute new speeds   
+                computeNewSpeeds();
+        
+                // limit checks
+                limitChecks();   
+                
+                checkForIntersection();  
+                
+                if(foundIntersection){
+                    CurrentLineFollowerState = STATE_STOP;
+                }else if(!foundIntersection){
+                    CurrentLineFollowerState = STATE_FORWARD;
+                }
+                
+                m3pi.cls();
+                
+                break;
+            }
+            case STATE_FORWARD:
+            {           
+                goForwards();
+                CurrentLineFollowerState = STATE_COMPUTE;
+                break;
+            }
+            case STATE_LEFT:
+            {
+                turnLeft90();
+                CurrentLineFollowerState = STATE_COMPUTE;
+                break;
+            }
+            case STATE_RIGHT:
+            {
+                turnRight90();
+                CurrentLineFollowerState = STATE_COMPUTE;
+                break;
+            }
+            case STATE_BACKWARD:
+            {
+                CurrentLineFollowerState = STATE_COMPUTE;
+                break;
+            }
+            case STATE_STOP:
+            {
+                m3pi.stop();
+                CurrentLineFollowerState = STATE_VLC;
+                break;
+            }
+            case STATE_VLC:
+            {
+                m3pi.cls();
+                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;
+                
+                int index = decoder.decode();
+                wait(1); //wait 1 sec before executing selected direction
+                switch(index){
+                    case 0:{ //links
+                        CurrentLineFollowerState = STATE_LEFT;
+                        break;
+                    }
+                    case 1:{ //rechts
+                        CurrentLineFollowerState = STATE_RIGHT;
+                        break;
+                    }
+                    case 2:{ //rechtdoor
+                         CurrentLineFollowerState = STATE_FORWARD;
+                        break;
+                    }
+                    case 3:{ //achteruit
+                        CurrentLineFollowerState = STATE_BACKWARD;
+                        break;
+                    }
+                }
+            }
+        }
+    }
+    
+    float LineFollower::computeDerivative(void){
+        return derivative = current_pos_of_line - previous_pos_of_line;
+    }
+    
+    float LineFollower::computeIntegral(void){
+         return integral += proportional;
+    }
+    
+    float LineFollower::computePower(void){
+        return power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+    }
+    
+    void LineFollower::computeNewSpeeds(void){
+        right = speed+power;
+        left  = speed-power;
+    }
+    
+    void LineFollower::limitChecks(void){
+        if (right < MIN_SPEED)
+            right = MIN_SPEED;
+        else if (right > MAX_SPEED)
+            right = MAX_SPEED;
+            
+        if (left < MIN_SPEED)
+            left = MIN_SPEED;
+        else if (left > MAX_SPEED)
+            left = MAX_SPEED;
+    }
+    
+    void LineFollower::turnLeft90(void){
+        m3pi.left_motor(MAX_SPEED);
+        m3pi.right_motor(MAX_SPEED);
+        wait(MAX_SPEED);
+        m3pi.stop();
+        m3pi.left(MAX_SPEED);
+        wait(MAX_SPEED*1.5);
+    }
+    
+    void LineFollower::turnRight90(void){
+        m3pi.left_motor(MAX_SPEED);
+        m3pi.right_motor(MAX_SPEED);
+        wait(MAX_SPEED);
+        m3pi.stop();
+        m3pi.right(MAX_SPEED);
+        wait(MAX_SPEED*1.5);
+    }
+    
+    void LineFollower::goBackwards(void){
+        m3pi.left_motor(-left);
+        m3pi.right_motor(-right);
+    }
+    
+    void LineFollower::goForwards(void){
+        m3pi.left_motor(left);
+        m3pi.right_motor(right);
+    }
+    
+    void 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;
+        }else{
+            foundIntersection = false;
+        }
+    }
+    
+}
\ No newline at end of file