File content as of revision 3:4ba3d22e50dc:

#include "mbed.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;

float currSlope;

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

int main() {
    int integrationCounter = 0;
    
    while(1) {
            
        if(integrationCounter % 461== 0){
            si = 1;
            clk = 1;
            //wait(.00001);
            si = 0;
            clk = 0;
            integrationCounter = 0;
            
            slopeAccum = 0;
            slopeCount = 0;
            approxPos = 0;
            peak1 = 0;
            peak2 = 0;
                
        }
        else if (integrationCounter > 129){
            for (int c = 0; c < 128; c++) {
                if (ADCdata[c+1] < ADCdata[c] && !peak1){
                    peak1 = 1;
                    minVal = ADCdata[c];
                } else if (ADCdata[c] < minVal && !peak2){
                    minVal = ADCdata[c];
                } else if (ADCdata[c+1] < ADCdata[c] && !peak2){
                    peak2 = 1;
                }
            }
            for (int c = 0; c < 128; c++) {
                if (ADCdata[c] > minVal && ADCdata[c] - minVal < 0.05f){
                    slopeAccum += c;
                    slopeCount++;
                }
            }
            approxPos = (float)slopeAccum/(float)slopeCount;
            
            if(approxPos > 0 && approxPos <= 64){
                servo.pulsewidth(hardRight);
            } else if (approxPos > 64 && approxPos <= 128) {
                servo.pulsewidth(slightRight);
            }
            
            /*
            if(approxPos > 0 && approxPos <= 16){
                servo.pulsewidth(hardRight);
            } else if (approxPos > 16 && approxPos <= 32) {
                servo.pulsewidth(slightRight);
            } else if (approxPos > 32 && approxPos <= 96) {
                servo.pulsewidth(straight);
            } else if (approxPos > 96 && approxPos <= 112) {
                servo.pulsewidth(slightLeft);
            } else if (approxPos > 112 && approxPos <= 128) {
                servo.pulsewidth(hardLeft);
            }
            */
            integrationCounter = 460;
        }
        else{
            clk = 1;
            wait(.0000001);
            ADCdata[integrationCounter - 1] = camData;
            clk = 0;
        }

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