File content as of revision 2:21fb894dc9d6:

#include "mbed.h"
#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;
        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;
        vlcDecoder = Manchester();
    }
    
    void LineFollower::followTheLine(void){
        switch(CurrentLineFollowerState){              
            case STATE_INIT:
            {
                m3pi.sensor_auto_calibrate();
                CurrentLineFollowerState = STATE_COMPUTE;
                break;
            }
            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();
        
                // Remember the last position.
                previous_pos_of_line = current_pos_of_line;
        
                // Compute the power
                computePower();
        
                // Compute new speeds   
                computeNewSpeeds();
        
                // limit checks
                limitChecks();   
            
                if(checkForIntersection()){
                    CurrentLineFollowerState = STATE_STOP;
                }else if(!checkForIntersection()){
                    CurrentLineFollowerState = STATE_FORWARD;
                }
                
                m3pi.cls();
                
                break;
            }
            case STATE_FORWARD:
            {           
                goForwards();
                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();
                CurrentLineFollowerState = STATE_COMPUTE;
                break;
            }
            case STATE_RIGHT:
            {
                turnRight90();
                CurrentLineFollowerState = STATE_COMPUTE;
                break;
            }
            case STATE_TURN180:
            {
                CurrentLineFollowerState = STATE_COMPUTE;
                turnRight90();
                turnRight90();
                break;
            }
            case STATE_STOP:
            {
                m3pi.stop();
                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:
            {
                m3pi.cls();
                m3pi.locate(0,0);
                m3pi.printf("VLC");
                m3pi.locate(0,1);
                m3pi.printf("Activate");
                wait(1);
                
                //retry decoding if detected value is equal to 5 = faulty stop
                    direction_index = vlcDecoder.decode();
                }while(direction_index == 5);
                
                wait(0.5);
                m3pi.cls();
                m3pi.printf("DIR fnd");
                
                wait(1); //wait 1 sec before executing selected direction
                switch(direction_index){
                    case 0:{ //left
                        CurrentLineFollowerState = STATE_LEFT;
                        ledLinks = 1;
                        break;
                    }
                    case 1:{ //right
                        CurrentLineFollowerState = STATE_RIGHT;
                        ledRechts = 1;
                        break;
                    }
                    case 2:{ //forwards
                        CurrentLineFollowerState = STATE_LONG_FORWARD;
                        ledRechtdoor = 1;
                        break;
                    }
                    case 3:{ //backwards
                        CurrentLineFollowerState = STATE_TURN180;
                        ledAchteruit = 1;
                        break;
                    }
                    case 4:{ //ignore
                        CurrentLineFollowerState = STATE_LONG_FORWARD;
                        break;
                    }
                    default:{
                        CurrentLineFollowerState = STATE_LONG_FORWARD;
                        m3pi.cls();
                    }
                }
                
                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::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);  
        
        //check 2 outermost IR sensors on both sides
        if((values[0] > 1900 && values[1] > 1900) || (values[3] > 1900 && values[4] > 1900)){
            return true;
        }else{
            return false;
        }
    }
    
}