File content as of revision 0:4bb3dfde1219:

#include "mbed.h"
#include "variables.h"
#include "SRF05.h"

DigitalOut myled(LED1);
#include <vector> //Tracker data


#define TESTING 1  //for TDD


// Forwards and backwards
void driveWheels(int direction, float pause)
{
    
#if TESTING
    pc.printf("Driving\n");
#endif
    
    if(direction == forward)
    {
        drive   = 1;
        turnRight     = 0;
        wait(pause);        
    }
    else
    {
        drive   = 0;
        turnRight     = 1;
        wait(pause);  
    }
    drive   = 0;
    turnRight     = 0;
    
#if TESTING
    wait(0.01);
#endif
}

// Steering function
void turnWheels(int direction)
{
#if TESTING
    pc.printf("Turning\n");
#endif
    switch(direction)
    {
        case left:
            turnLeft = 0;
            turnRight = 1;
            break;
        case right:
            turnLeft = 1;
            turnRight = 0;
            break;  
        case center:
        defauturnLeft:
            turnLeft = 0;
            turnRight = 0;
            break;
    }
}

//Robot location
std::vector <float> readPos()
{
#if TESTING
    pc.printf("Reading\n");
#endif
    std::vector<float> pos;
    pos.push_back(ir1.read());
    pos.push_back(ir2.read());
    pos.push_back(ir3.read());
    pos.push_back(ir4.read());
    pos.push_back(ir5.read());
    return pos;
}

int wheelDir(std::vector<float> line)
{
#if TESTING
    pc.printf("Locating\n");
#endif
    int ret = 100;
    float low = 1;
    for (unsigned i=0; i<line.size(); ++i)
    {
#if TESTING
    pc.printf("%.1f", line[i]);
    pc.printf("\n");
#endif
        if(line[i] < low && line[i] < 0.7)
        {
            ret = i; 
            low = line[i];
        }
       
    }
        
#if TESTING
    pc.printf("%u", ret);
    pc.printf("\n");
#endif
    return ret;
}
//Collision handling
float collision(float col){
    col = srf.read();
#if TESTING    
    pc.printf("Distance = %.1f\n", col);
#endif    
    return col;
}

//Main Loop
int main()
{
#if TESTING
    pc.printf("Set_up_variables\n");
#endif
    std::vector <float>line;
    std::vector <float> pturnRightLine = readPos();
    int dir;
    int pos;
    float col;
    while(1)
    {
#if TESTING
        pc.printf("Main\n");
#endif
        line = readPos();
        pos = wheelDir(line);
#if TESTING
        pc.printf("%u",pos);
#endif
        
        if(pos == 100) 
        { 
            driveWheels(backward, fulldrive);
        }
        else 
        {
            if(pturnRightLine != line)
            {
                switch(pos) 
                {
                    case 0:
                    case 1:
                        dir = left;
                        break; 
                    case 2: 
                        dir = center;
                        break;
                    case 3:
                    case 4:
                        dir = right;
                        break;   
                }
                pturnRightLine.clear();
                std::vector<float> pturnRightLine(line);     
                turnWheels(dir);
                collision(col);    
            }
            driveWheels(forward, fulldrive);
            //Collision handling
            if(col <= 200){
                driveWheels(backward, fulldrive);
            }      
        } 
    }
}