File content as of revision 4:a8dce9e269e5:

#include "mbed.h"
#include "stdlib.h"

DigitalOut clk(PTA13);
DigitalOut si(PTD4);
AnalogIn camData(PTC2);
PwmOut servo(PTA5);
Serial pc(USBTX, USBRX); // tx, rx

float ADCdata [128];
float slopeAccum;
float slopeCount;
float approxPos;
float minVal;

int peak1;
int peak2;

int peakLoc1;
int peakLoc2;

int risEdge;

float currSlope;

float straight = 0.00155f;
float hardLeft = 0.0013f;
float slightLeft = 0.00145f;
float hardRight = 0.0018f;
float slightRight = 0.00165f;

float currDirection = straight;

int comp (const void * elem1, const void * elem2) 
{
    int f = *((int*)elem1);
    int s = *((int*)elem2);
    if (f > s) return  1;
    if (f < s) return -1;
    return 0;
}



int main() {
    //servo.period(SERVO_FREQ);
    int integrationCounter = 0;
    
    while(1) {
            
        if(integrationCounter % 151== 0){
            si = 1;
            clk = 1;
            //wait(.00001);
            si = 0;
            clk = 0;
            integrationCounter = 0;
            
            slopeAccum = 0;
            slopeCount = 0;
            approxPos = 0;
            peak1 = 0;
            peak2 = 0;
            risEdge = 0;
            peakLoc1 = 0;
            peakLoc2 = 0;
                
        }
        else if (integrationCounter > 129){
            minVal = 0;
            for (int c = 0; c < 126; c++) {
                if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && !peak1 && !risEdge && !peak2){
                    peak1 = 1;
                    peakLoc1 = c;
                    minVal = ADCdata[c];
                } else if (ADCdata[c] < minVal && peak1 && !peak2){
                    minVal = ADCdata[c];
                } else if (ADCdata[c+1] > ADCdata[c] && ADCdata[c+2] > ADCdata[c+1] && peak1 && !peak2){
                    risEdge = 1;
                } else if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && peak1 && risEdge && !peak2){
                    peak2 = 1;
                    peakLoc2 = c;
                }
            }
            
            for (int c = peakLoc1; c < peakLoc2; c++) {
                if (ADCdata[c] > minVal && ADCdata[c] - minVal < 0.01f && ADCdata[c] > 0.1f){
                    slopeAccum += c;
                    slopeCount++;
                }
            }
            
            approxPos = (float)slopeAccum/(float)slopeCount;
            
            if (peak1 && peak2){
                if(approxPos > 0 && approxPos <= 32){
                    if (currDirection != slightLeft){
                        currDirection = slightLeft;
                        servo.pulsewidth(slightLeft);
                    }
                } else if (approxPos > 32 && approxPos <= 60){
                    if (currDirection != straight){
                        currDirection = straight;
                        servo.pulsewidth(straight);
                    }
                } else if (approxPos > 70 && approxPos <= 128){
                    if (currDirection != slightRight){
                        currDirection = slightRight;
                        servo.pulsewidth(slightRight);
                    }
            } else if (approxPos < 32){
                if (currDirection != hardLeft){
                        currDirection = hardLeft;
                        servo.pulsewidth(hardLeft);
                }
            }
        }
            integrationCounter = 150;
        }
        else{
            clk = 1;
            wait_us(50);
            ADCdata[integrationCounter - 1] = camData;
            clk = 0;
        }

        //clk = 0;
        integrationCounter++;
        //camData.
        
    }
}