File content as of revision 0:1f5782fc5ca3:

#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;
        }
    }
    
}