File content as of revision 23:d8cacd996cce:

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

// Camera Pins
DigitalOut clk(PTA13);
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);

//Telemetry
MODSERIAL telemetry_serial(PTA2, PTA1);
telemetry::MbedHal telemetry_hal(telemetry_serial);
telemetry::Telemetry telemetry_obj(telemetry_hal);
telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
telemetry::Numeric<float> tele_motor_pwm(telemetry_obj, "motor1", "Motor PWM", "%DC", 0);


// 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){
    tele_motor_pwm.set_limits(0, 1);
    telemetry_obj.transmit_header();
    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){
               //uint16_t * data = camData.read_u16();
               for (uint16_t i =0; i < 128; i++) {
                   tele_linescan[i] = camData.read_u16();
               }
                telemetry_obj.do_io(); 
                motor1.write(tele_motor_pwm);
                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);
    }
    
    
}