File content as of revision 7:11dd5581c763:

#include <mbed.h>
#include <funcdef.h>
//#include <iostream>
#include <math.h>

float fmax(float x, float y)
{
    if(x > y)
    {
        return x;
    }else
    {
        return y;
    }
}  

float interpolation(float errorVar)
{
    float distanceMm=0.0;
    for (int i=1;i<=105;i++) //not looking at first values, because interpolation occurs within range of values
    {        
        if ((errorVar>=linearOffset[i][0]) && (errorVar<linearOffset[i-1][0]))
        {
            //printf("linearOffset %8.4f", linearOffset[i-1][0]);
            //printf("linearOffset %8.4f\r\n", linearOffset[i][0]); 
            distanceMm=linearOffset[i][1]+(errorVar-linearOffset[i][0])*((linearOffset[i-1][1]-linearOffset[i][1])/(linearOffset[i-1][0]-linearOffset[i][0]));
        }
    }
    return distanceMm;
}

void senseFunc()
{
    //READ ALL SENSORS
    float totalError = 0.0;
  
    //for (int i=0;i<=7;i++)
    for (int i=7;i>=0;i--)
    {
        sensorValue[i]=((sensorPin[i]).read_u16()>>4); 
        sensorErrors[i]=((1/(float)(sensorMaxValue[i]-sensorMinValue[i]))*sensorValue[i])+(1-(sensorMaxValue[i]/(float)(sensorMaxValue[i]-sensorMinValue[i])));
    }    
    
    
    if ((sensorErrors[6]<=(float)0.7) && (sensorErrors[7]<=(float)0.7))
    {
        totalError = (fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2+fmax(sensorErrors[4],0.0)*3+fmax(sensorErrors[6], 0.0)*5) - 
        (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2+fmax(sensorErrors[5],0.0)*3+fmax(sensorErrors[7], 0.0)*5);
    }
    else
    {    
        //To compensate for the black lines on the ramp
        totalError = (fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2) - 
        (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2);
        accumulator=0;
        errorDetectthres=100;
        LEDVal=0;
    }
       
    errorVar = -totalError;
    errorVar=interpolation(errorVar)/-3.81;
 
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    for (int i=0;i<=7;i++)
    {
        if ((sensorValue[i])>=(sensorThreshold[i]))
        {
            sensorErrorsdiscrete[i] = 1;
        }
        else
        {
            sensorErrorsdiscrete[i] = 0;
        }
    }
    
    if ((sensorErrorsdiscrete[0]) == 1)
    {
        if ((sensorErrorsdiscrete[1]) == 1)
        {
            errorVardiscrete=0;
        }
        else if ((sensorErrorsdiscrete[2]) == 1)
        {
            errorVardiscrete=-1;
        }  
        else
        {
            errorVardiscrete=-0.5;
        }
    }       
    else if ((sensorErrorsdiscrete[1]) == 1)
    {
        if ((sensorErrorsdiscrete[0]) == 1)
        {
            errorVardiscrete=0;
        }
        else if ((sensorErrorsdiscrete[3]) == 1)
        {
            errorVardiscrete=1;
        }  
        else
        {
            errorVardiscrete=0.5;
        }    
    }   
    else if ((sensorErrorsdiscrete[2]) == 1)
    {
        if ((sensorErrorsdiscrete[0]) == 1)
        {
            errorVardiscrete=-1;
        }
        else if ((sensorErrorsdiscrete[4]) == 1)
        {
            errorVardiscrete=-2;
        }  
        else
        {
            errorVardiscrete=-1.5;
        }    
    }  
    else if ((sensorErrorsdiscrete[3]) == 1)
    {
        if ((sensorErrorsdiscrete[1]) == 1)
        {
            errorVardiscrete=1;
        }
        else if ((sensorErrorsdiscrete[5]) == 1)
        {
            errorVardiscrete=2;
        }  
        else
        {
            errorVardiscrete=1.5;
        }    
    } 
    else if ((sensorErrorsdiscrete[4]) == 1)
    {
        if ((sensorErrorsdiscrete[2]) == 1)
        {
            errorVardiscrete=-2;
        }
        else if ((sensorErrorsdiscrete[6]) == 1)
        {
            errorVardiscrete=-3.5;
        }  
        else
        {
            errorVardiscrete=-2.5;
        }    
    } 
    else if ((sensorErrorsdiscrete[5]) == 1)
    {
        if ((sensorErrorsdiscrete[3]) == 1)
        {
            errorVardiscrete=2;
        }
        else if ((sensorErrorsdiscrete[7]) == 1)
        {
            errorVardiscrete=3.5;
        }  
        else
        {
            errorVardiscrete=2.5;
        }    
    } 
    else if ((sensorErrorsdiscrete[6]) == 1)
    {
        if ((sensorErrorsdiscrete[4]) == 1)
        {
            errorVardiscrete=-3.5;
        }         
        else
        {
            //errorVardiscrete=-4;
            errorVardiscrete=-5;
        }    
    }
    else if ((sensorErrorsdiscrete[7]) == 1)
    {
        if ((sensorErrorsdiscrete[5]) == 1)
        {
            errorVardiscrete=3.5;
        }         
        else
        {
            //errorVardiscrete=4;
            errorVardiscrete=5;
        }    
    }
    else
    {
        errorVardiscrete=previousErrordiscrete;
    }
    
    if (errorVar>largestpositiveerrorvar)
    {
        largestpositiveerrorvar=errorVar;
    }
    if (errorVar<largestnegativeerrorvar)
    {
        largestnegativeerrorvar=errorVar;
    }    
    
    if (((errorVar)<0.2) && ((errorVardiscrete)>2))
    {
        errorVar=largestpositiveerrorvar;
    }   
    if (((errorVar)<0.2) && ((errorVardiscrete)<-2))
    {
        errorVar=largestnegativeerrorvar;
    } 
      
    //printf("errorVardiscrete: %d   ",errorVardiscrete);  
    //printf("errorVar:  %f\r\n",errorVar);   
    //wait_ms(200);
    return;
}