File content as of revision 5:60560dd127a2:

#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;

float peakVal1;
float peakVal2;

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 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;
            peakVal1 = 0;
            peakVal2 = 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;
                    peakVal1 = ADCdata[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;
                    peakVal2 = ADCdata[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 <= 40){
                    if (peakVal2 - peakVal1 > 0.65f){
                        servo.pulsewidth(hardLeft);
                    } else {
                        servo.pulsewidth(slightLeft);
                    }
                } else if (approxPos > 40 && approxPos <= 60){
                    servo.pulsewidth(straight);
                } else if (approxPos > 70 && approxPos <= 128){
                    if (peakVal1 - peakVal2 > 0.8f){
                        servo.pulsewidth(hardRight);
                    } else {
                        servo.pulsewidth(slightRight);
                    }
                }
            }
            integrationCounter = 150;
        }
        else{
            clk = 1;
            wait_us(50);
            ADCdata[integrationCounter - 1] = camData;
            clk = 0;
        }

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