File content as of revision 20:53f5e6d3e47b:

#include "mbed.h"
#include "rtos.h"
#include "telemetry.h"

// Camera Pins
DigitalOut clk(PTD5);
DigitalOut si(PTD4);
AnalogIn camData(PTC2);
//Servo, Motor, Serial Pins
PwmOut servo(PTA5);
PwmOut motor1(PTA4);
PwmOut motor2(PTA12);
DigitalOut break1(PTC12);
DigitalOut break2(PTC13);
Serial pc(USBTX, USBRX);
// Interrupt for encoder
InterruptIn interrupt(PTA13);
const int maxFrames = 3;
float frames[maxFrames][128];
float ADCdata [128];
Mutex line_mutex;


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


void linecam_thread(void const *args){
    while(1){
        //line_mutex.lock();
        pc.printf("[");
        for(int i=0; i<128; i++){
            //pc.printf("%f",ADCdata[i]);
            pc.printf(" ");
        }
        pc.printf("]");
        //line_mutex.unlock();
    }
    
}

int find_track(float line[]){
    int track_location = -1;
    float slope_threshold = .05;
    bool downslope_indices [128] = {false};
    bool upslope_indices [128] = {false};
    for(int i=10; i<118; i++){
        if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
            downslope_indices[i] = true;
        }
        if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
            upslope_indices[i] = true;
        }
    }
    int numDownslopes = 0;
    int numUpslopes = 0;
    for(int i=0; i<128; i++){
        if(downslope_indices[i] == true){
            numDownslopes ++;
        }
        if(upslope_indices[i] == true){
            numUpslopes ++;
        }
    }
    int downslope_locs [numDownslopes];
    int upslope_locs [numUpslopes];
    int dsctr = 0;
    int usctr = 0;
    
    for (int i=0; i<128; i++){
        if(downslope_indices[i] == true){
            downslope_locs[dsctr] = i;
            dsctr++;
        }
        if(upslope_indices[i] == true){
            upslope_locs[usctr] = i;
            usctr++;
        }
    }
    
    for(int i=0; i<numDownslopes; i++){
        for(int j=0; j<numUpslopes; j++){
            if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
                track_location = downslope_locs[i] + 2 ;
            }
        }
    }
    
    pc.printf("Downslopes at: ["); 
    for(int i = 0; i<numDownslopes; i++){
        pc.printf("%i ", downslope_locs[i]);
        
    }
    pc.printf("]\n\r");
    pc.printf("Upslopes at: [");
    for(int i=0; i<numUpslopes; i++){
        pc.printf("%i ", upslope_locs[i]);
    }
    pc.printf("]\n\r");
    return track_location;
}

int main() {
    //Thread thread(linecam_thread);
    //thread.set_priority(osPriorityIdle);
    pc.printf("Beginning linecam data acquisition... \n\r");
    for(int numFrames =  0; numFrames < maxFrames; numFrames++){
        for(int integrationCounter = 0;integrationCounter < 151;) {
            if(integrationCounter % 151== 0){
                si = 1;
                clk = 1;
                //wait(.00001);
                si = 0;
                clk = 0;
                integrationCounter = 0;
               
                    
            }
            else if (integrationCounter > 129){
                
                
                integrationCounter = 150;
            }
            else{
                clk = 1;
                wait_us(50);
                //line_mutex.lock();
                frames[numFrames][integrationCounter - 1] = 1-camData; // 1- is for light line
                //line_mutex.unlock();
                clk = 0;
            }
    
            integrationCounter++;
            
        }
        //frames[numFrames] = ADCdata;
    }
    
    for(int i =0; i<maxFrames; i++){
        pc.printf("LINE %i: [",i);
        
        
        for(int j = 0; j < 128; j++){
            pc.printf(" %f",frames[i][j]);
        }
        pc.printf("]\n\r");
        float* line = frames[i];
        int track = find_track(line);
        printf("track at %i\n\r",track);
    }
    
    
}