File content as of revision 7:11dd5581c763:

#include <mbed.h>
#include <funcdef.h>
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Initialise Global Variables

int sensorValue[] = {0,0,0,0,0,0,0,0};

int sensorMinValue[] = {0,0,0,0,0,0,0,0};
int sensorMaxValue[] = {0,0,0,0,0,0,0,0};

int sensorMinValue1[] = {0,0,0,0,0,0,0,0};
int sensorMaxValue1[] = {0,0,0,0,0,0,0,0};

int sensorMinValue2[] = {0,0,0,0,0,0,0,0};
int sensorMaxValue2[] = {0,0,0,0,0,0,0,0};

int sensorThreshold[] = {0,0,0,0,0,0,0,0};
int sensorErrorsdiscrete[] = {0,0,0,0,0,0,0,0};
float sensorErrors[] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};

float linearOffset[106][2]={
{   6.8275  ,   -26.0300    },
{   6.6497  ,   -25.4700    },
{   6.4960  ,   -24.9800    },
{   6.2632  ,   -24.4900    },
{   5.9921  ,   -23.9900    },
{   5.8196  ,   -23.4700    },
{   5.7159  ,   -22.9600    },
{   5.5201  ,   -22.4600    },
{   5.4829  ,   -22.0000    },
{   5.3919  ,   -21.4900    },
{   5.2843  ,   -20.9700    },
{   5.2686  ,   -20.4500    },
{   5.2052  ,   -19.9900    },
{   5.1198  ,   -19.5200    },
{   5.0410  ,   -19.0000    },
{   4.9476  ,   -18.4500    },
{   4.8565  ,   -18.0300    },
{   4.6842  ,   -17.5100    },
{   4.5375  ,   -16.9800    },
{   4.4060  ,   -16.4400    },
{   4.3064  ,   -16.0200    },
{   4.1894  ,   -15.4500    },
{   3.9800  ,   -14.9600    },
{   3.8762  ,   -14.5200    },
{   3.7609  ,   -13.9800    },
{   3.6261  ,   -13.4800    },
{   3.5013  ,   -13.0300    },
{   3.3357  ,   -12.4800    },
{   3.3057  ,   -12.0000    },
{   3.2587  ,   -11.4900    },
{   3.2009  ,   -11.0000    },
{   3.1255  ,   -10.4100    },
{   3.0409  ,   -9.9600 },
{   2.9758  ,   -9.4600 },
{   2.8237  ,   -9.0000 },
{   2.7329  ,   -8.4500 },
{   2.5944  ,   -7.9500 },
{   2.4453  ,   -7.4300 },
{   2.1612  ,   -6.9500 },
{   2.1333  ,   -6.5100 },
{   1.9384  ,   -6.0200 },
{   1.7274  ,   -5.4800 },
{   1.4727  ,   -4.9600 },
{   1.2838  ,   -4.4900 },
{   1.0132  ,   -3.9800 },
{   0.7237  ,   -3.3500 },
{   0.6065  ,   -3.0000 },
{   0.4716  ,   -2.4700 },
{   0.3420  ,   -1.9700 },
{   0.2302  ,   -1.4900 },
{   0.1598  ,   -0.9700 },
{   0.0632  ,   -0.4900 },
{   -0.0137 ,   0.0000  },
{   -0.0542 ,   0.4800  },
{   -0.1547 ,   1.0000  },
{   -0.1795 ,   1.4900  },
{   -0.2608 ,   1.9900  },
{   -0.3267 ,   2.5000  },
{   -0.4297 ,   2.9900  },
{   -0.5448 ,   3.5600  },
{   -0.6416 ,   4.0900  },
{   -0.8621 ,   4.5000  },
{   -1.0357 ,   5.0000  },
{   -1.2421 ,   5.4700  },
{   -1.5076 ,   6.0300  },
{   -1.8054 ,   6.5500  },
{   -2.0865 ,   7.0300  },
{   -2.2763 ,   7.4700  },
{   -2.5454 ,   8.0000  },
{   -2.7537 ,   8.5600  },
{   -2.8835 ,   8.9900  },
{   -3.0262 ,   9.5000  },
{   -3.2092 ,   10.0000 },
{   -3.3651 ,   10.5600 },
{   -3.4420 ,   10.9900 },
{   -3.5348 ,   11.5000 },
{   -3.5697 ,   11.9900 },
{   -3.6499 ,   12.5300 },
{   -3.7035 ,   13.0000 },
{   -3.7264 ,   13.4700 },
{   -3.8294 ,   14.0100 },
{   -3.9171 ,   14.5100 },
{   -4.0666 ,   15.1100 },
{   -4.1025 ,   15.4700 },
{   -4.3145 ,   16.0300 },
{   -4.4127 ,   16.4800 },
{   -4.5864 ,   17.0500 },
{   -4.6950 ,   17.4700 },
{   -4.8164 ,   18.0300 },
{   -4.9033 ,   18.5300 },
{   -5.0180 ,   19.0200 },
{   -5.1011 ,   19.5400 },
{   -5.2195 ,   20.0200 },
{   -5.3131 ,   20.4700 },
{   -5.3823 ,   21.0200 },
{   -5.4816 ,   21.5200 },
{   -5.5606 ,   21.9800 },
{   -5.6365 ,   22.5000 },
{   -5.7055 ,   23.0900 },
{   -5.7591 ,   23.4700 },
{   -5.9464 ,   23.9900 },
{   -6.1486 ,   24.5400 },
{   -6.3176 ,   25.0000 },
{   -6.4417 ,   25.5000 },
{   -6.6916 ,   25.9800 },
{   -6.9624 ,   26.5600 }};

float motorLookup[11][2]={
{   0.000   ,   0.000   },
{   20.000  ,   100.000 },
{   300.000 ,   200.000 },
{   520.000 ,   300.000 },
{   660.000 ,   400.000 },
{   750.000 ,   500.000 },
{   820.000 ,   600.000 },
{   860.000 ,   700.000 },
{   920.000 ,   800.000 },
{   940.000 ,   900.000 },
{   1000.000,   1000.000}};

float Prop=0;
float Inte=0;
float Dere=0;

float errorVar=0;
float largestpositiveerrorvar=0;
float largestnegativeerrorvar=0;
int errorVardiscrete=0;
int previousErrordiscrete=0; 
float previousError=0; 
float PIDs=0;
int newPIDs=0;
int accumulator=0;

bool stopMotors=false;

int leftMotorValTemp=0;
int rightMotorValTemp=0;

int initialMotorspeed = 550;//IF THIS IS CHANGED, CHANGE IT IN TICKER
int errorDetectthres=100;//IF THIS IS CHANGED, CHANGE IT IN BLACK ENDGE DETECTION

Ticker ticker1;
Ticker ticker2;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Main Code

void tracker() {
    //IF THE ERROR IS EQUAL TO THE LAST ERROR, START A COUNTING VARIABLE AND ADD THIS TO THE NOMINAL MOTOR SPEED (IE WHEN ERROR OF 0), IF CASE NOT TRUE, DO NOTHING/RETURN TO NORMAL SPEED FOR CORNER.
    if (abs(PIDs)<=errorDetectthres)
    {
        accumulator++;
        LEDVal=1;        
    }
    else
    {
        accumulator=0;
        errorDetectthres=65;
        LEDVal=0;
    }
    
    if (accumulator>=14)
    {
        initialMotorspeed = 975;
    }
    else if (accumulator>=12)
    {
        initialMotorspeed = 975;
        errorDetectthres=300;
    }
    else if (accumulator>=10)
    {
        initialMotorspeed = 950;
    }
    else if (accumulator>=8)
    {
        initialMotorspeed = 900;
        errorDetectthres=150;
    }
    else if (accumulator>=6)
    {
        initialMotorspeed = 600;
        errorDetectthres=125;
    }
    else if (accumulator>=4)
    {
        initialMotorspeed = 550;
    }
    else if (accumulator>=2)
    {
        initialMotorspeed = 550;
    }
    else
    {
        initialMotorspeed = 550;
    }
}

void calculations()
{
    //USE ERROR TO CALCULATE PID VALUES
    Prop = errorVar;
    Inte = Inte + errorVar;
    Dere = ((errorVar) - (previousError));
    
    //BECAUSE THE CURRENT ERROR IS CALCULATED IN A FUNCTION BEFORE THIS, ON THE NEXT ITERATION WE WANT TO REMEMBER THE PREVIOUS VARIABLE
    previousError=errorVar;
    previousErrordiscrete=errorVardiscrete;
    
    //FORMULATE OFFSET VALUE FROM ERROR, PID VALUES AND CONSTANTS
    PIDs = (Kp*Prop)+(Kd*Dere)+(Ki*Inte);     
    newPIDs=PIDs;
}

int main() 
{    
    //CONFIGURE MOTORS & TURN OFF LED    
    LEDVal=0;
    leftMotorVal.period_us(1000);
    rightMotorVal.period_us(1000);
    leftMotorVal.pulsewidth_us(0); 
    rightMotorVal.pulsewidth_us(0);
    
    wait_ms(1000); 
    
    //TURN LED ON
    LEDVal=1;    
 
    //BEGIN CALIBRATION
    CalibrateFunc();

    //TURN OFF LED AND SET PULSES TO 2 HZ
    LEDVal=0; 
   
    //FIND LINE
    findLineFunc();    

    //ONCE FOUND, HAND OVER TO MAIN ALGORITHM 
    ticker1.attach(&tracker, 0.2);
    ticker2.attach(&calculations, 0.002);
    while(1)
    {     
        senseFunc();
        CountingVar=!CountingVar;
        //calculateFunc();
        driveFunc();        
    }
    
    return 0;
}