File content as of revision 16:79106efd7a57:

#include "mbed.h"
#include "stdlib.h"
#include <vector>
//Outputs
DigitalOut led1(LED1);
DigitalOut clk(PTD5);
DigitalOut si(PTD4);
PwmOut motor1(PTA12);
PwmOut motor2(PTA4);
DigitalOut break1(PTC7);
DigitalOut break2(PTC0);
PwmOut servo(PTA5);

Serial pc(USBTX, USBRX); // tx, rx

//Inputs
AnalogIn camData(PTC2);

//Encoder setup and variables
InterruptIn interrupt(PTA13);

//Line Tracking Variables --------------------------------
float ADCdata [128];
float maxAccum;
float maxCount;
float approxPos;
float prevApproxPos;
int trackWindow = 30;
int startWindow;
int endWindow;
float maxVal;
int maxLoc;

bool firstTime;

//Data Collection
bool dataCol = false;
int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime;

//Line Crossing variables
int prevTrackLoc;
int spaceThresh = 1;
int widthThresh = 10;
bool space;

//Servo turning parameters
float straight = 0.00155f;
float hardLeft = 0.0012f;
float hardRight = 0.0020f;
//float hardLeft = 0.0010f;
//float hardRight = 0.00195f;

//Servo Directions
float currDir;
float prevDir;

// All linescan data for the period the car is running on. To be printed after a set amount of time
//std::vector<std::vector<int> > frames;
const int numData = 1000;
int lineCenters [numData] = {0};
int times [numData] = {0};
int loopCtr = 0;

//End of Line Tracking Variables -------------------------

//Encoder and Motor Driver Variables ---------------------

//Intervals used during encoder data collection to measure velocity
int interval1=0;
int interval2=0;
int interval3=0;
int avg_interval=0;
int lastchange1 = 0;
int lastchange2 = 0;
int lastchange3 = 0;
int lastchange4 = 0;

//Variables used to for velocity control
float avg_speed = 0;
float last_speed = 0;

float stall_check = 0;
float tuning_val = 1;

int numInterrupts = 0;

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~PWM & Integration Time ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
float pulsewidth = 0.20f;
int intTimMod = 20;
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
bool turnSpeedControl = true;

// Hardware periods
float motorPeriod = .0025;
float servoPeriod = .0025;

Timer t;
Timer servoTimer;
Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames

//Observed average speeds for each duty cycle
const float DESIRED_SPEED = 1;
const float TUNING_CONSTANT_10 = 1.90;
const float TUNING_CONSTANT_20 = 3.00;
const float TUNING_CONSTANT_30 = 4.30;
const float TUNING_CONSTANT_50 = 6.880;
const float PI = 3.14159;
const float WHEEL_CIRCUMFERENCE = .05*PI;

//Velocity Control Tuning Constants
const float TUNE_THRESH = 0.5f;
const float TUNE_AMT = 0.1f;

//Parameters specifying sample sizes and delays for small and large average speed samples
float num_samples_small = 3.0f;
float delay_small = 0.05f;
float num_samples_large = 100.0f;
float delay_large = 0.1f;

//Large and small arrays used to get average velocity values
float large_avg_speed_list [100];
float small_avg_speed_list [10];

//End of Encoder and Motor Driver Variables ----------------------

//Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

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

    return track_location;
}

//Function for speeding up KL25Z ADC
void initADC(void){
 
    ADC0->CFG1 = ADC0->CFG1 & (
        ~(
          0x80 // LDLPC = 0 ; no low-power mode
        | 0x60 // ADIV = 1
        | 0x10 // Sample time short
        | 0x03 // input clock = BUS CLK
        )
    ) ; // clkdiv <= 1
    ADC0->CFG2 = ADC0->CFG2 
        | 0x03 ; // Logsample Time 11 = 2 extra ADCK
    ADC0->SC3 = ADC0->SC3 
        & (~(0x03)) ; // hardware avarage off
}


// Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~

float get_speed(){
    float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
    float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
    return linearSpeed;
}

float get_avg_speed(float num_samples, float delay) {
    
    float avg_avg_speed = 0;
    
    for (int c = 0; c < num_samples; c++) {
        if (num_samples == num_samples_small){
            small_avg_speed_list[c] = get_speed();
        } else if (num_samples == num_samples_large){
            large_avg_speed_list[c] = get_speed();
        }
        //wait(delay);
        }
                
        for (int c = 1; c < num_samples; c++) {
            if (num_samples == num_samples_small){
                avg_avg_speed += small_avg_speed_list[c];
            } else if (num_samples == num_samples_large){
                avg_avg_speed += large_avg_speed_list[c];
            }
        }
    return avg_avg_speed/num_samples;
}

void velocity_control(float duty_cyc, float tuning_const) {
    
    avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
    
    //When determined speed is infinity or 0, set the speed to the last agreeable speed
    /*if (avg_speed > 100 || avg_speed == 0){
        avg_speed = last_speed;
    }*/
    
    pc.printf("\n\r%f", avg_speed);
    if (avg_speed == stall_check && tuning_const != 0 && avg_speed == 0) {
        avg_speed = 0;
        tuning_val += TUNE_AMT;
    } else if((avg_speed - tuning_const) > TUNE_THRESH){
        tuning_val -= TUNE_AMT;
        stall_check = avg_speed;

    } else if (tuning_const - avg_speed > TUNE_THRESH && avg_speed != 0){
        tuning_val += TUNE_AMT;
        stall_check = avg_speed;
    } else {
        //tuning_val = 1;
        stall_check = avg_speed;
    }
    
    if (tuning_val < .5){
        tuning_val = .5;
    }
    pc.printf("\n\rTuning Val: %f", tuning_val);
    motor1.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
    motor2.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
    
    if (avg_speed != 0){
        last_speed = avg_speed;
    }

}

// Interrupt Functions for Encoder ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~``
void fallInterrupt(){
    
    int time = t.read_us();
    interval1 = time - lastchange2;
    interval2 = lastchange1-lastchange3;
    interval3 = lastchange2 - lastchange4;
    avg_interval = (interval1 + interval2 + interval3)/3;
    
    lastchange4 = lastchange3;
    lastchange3 = lastchange2;
    lastchange2 = lastchange1;
    lastchange1 = time;
    
    numInterrupts++;
}

void riseInterrupt(){
    int time = t.read_us();
    interval1 = time - lastchange2;
    interval2 = lastchange1-lastchange3;
    interval3 = lastchange2 - lastchange4;
    avg_interval = (interval1 + interval2 + interval3)/3;
    
    lastchange4 = lastchange3;
    lastchange3 = lastchange2;
    lastchange2 = lastchange1;
    lastchange1 = time;
    
    numInterrupts++;
}


//End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

int main() {
    
    //Alter reg values to speed up KL25Z
    initADC();
    
    //Line Tracker Initializations
    int integrationCounter = 0;
    
    //Initial values for directions
    currDir = 0;
    prevDir = 0;
    
    // Motor Driver Initializations
    motor1.period(motorPeriod);
    motor2.period(motorPeriod);

    // Servo Initialization
    servo.period(servoPeriod);
    servo.pulsewidth(hardRight);
    wait(3);

    motor1.pulsewidth(motorPeriod*pulsewidth);
    motor2.pulsewidth(motorPeriod*pulsewidth);
    break1 = 0;
    break2 = 0;
    
    firstTime = true;
    
    //t.start();
    
    if(dataCol){
        loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
        printTimer.start();
    }

    while(1) {
        if(dataCol){
            //break out of main loop if enough time has passed;    
            if(loopCtr >= numData && dataCol){
                break;
            }
        }
        if(integrationCounter % 151== 0){
            /*
            //Disable interrupts
            interrupt.fall(NULL);
            interrupt.rise(NULL);
            */
            
            //Send start of integration signal
            si = 1;
            clk = 1;

            si = 0;
            clk = 0;
            
            //Reset timing counter for integration
            integrationCounter = 0;
            
            //Reset line tracking variables
            maxAccum = 0;
            maxCount = 0;
            approxPos = 0;
            
            space = false;

        }
        else if (integrationCounter > 129){
            //Start Timer
            //t.start();
            
            //Enable interrupts
            //interrupt.fall(&fallInterrupt);
            //interrupt.rise(&riseInterrupt);
            
            if (firstTime){
            
                maxVal = ADCdata[10];
                for (int c = 11; c < 118; c++) {
                    if (ADCdata[c] > maxVal){
                        maxVal = ADCdata[c];
                        maxLoc = c;
                    }
                }
                
                for (int c = 10; c < 118; c++) {
                        if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.01f){
                            maxAccum += c;
                            maxCount++;
                            if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
                                space = true;
                            }
                            prevTrackLoc = c;
                        }
                }
                
                //firstTime = false;
            } else {
                
                startWindow = prevApproxPos - trackWindow;
                endWindow = prevApproxPos + trackWindow;
                if (startWindow < 0){
                    startWindow = 0;
                }
                if (endWindow > 118){
                    endWindow = 118;
                }
                maxVal = ADCdata[10];
                for (int c = startWindow; c < endWindow; c++) {
                    if (ADCdata[c] > maxVal){
                        maxVal = ADCdata[c];
                        maxLoc = c;
                    }
                }
                
                for (int c = startWindow; c < endWindow; c++) {
                    if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.01f){
                        maxAccum += c;
                        maxCount++;
                        if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
                            space = true;
                        }
                        prevTrackLoc = c;
                    }
                }
            }
            /*
            //Check if we need to alter integration time due to brightness
            if (maxVal < 0.15f){
                intTimMod += 10;
            } else if (maxVal >= 1) {
                if (intTimMod > 0) {
                    intTimMod -= 10;
                }
            }
            */
            
            //Line Crossing Checks
            if (space) {
                currDir = prevDir;
                firstTime = true;
            } else {
                approxPos = (float)maxAccum/(float)maxCount;

                if(dataCol){
                    if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){
                        lineCenters[loopCtr] = approxPos;
                        times[loopCtr] = printTimer.read_ms();
                        loopCtr++;
                    }
                }
                
                currDir = hardLeft + (approxPos)/((float) 118)*(hardRight-hardLeft);
                prevApproxPos = approxPos;

            }

            if (turnSpeedControl){
                //Change speed when turning at different angles
                if(approxPos > 0 && approxPos <= 45){
                        motor1.pulsewidth(motorPeriod*(pulsewidth*0.8f));
                        motor2.pulsewidth(motorPeriod*(pulsewidth*0.8f));
                } else if (approxPos > 45 && approxPos <= 95){
                        motor1.pulsewidth(motorPeriod*pulsewidth);
                        motor2.pulsewidth(motorPeriod*pulsewidth);
                } else {
                    motor1.pulsewidth(motorPeriod*(pulsewidth*0.8f));
                    motor2.pulsewidth(motorPeriod*(pulsewidth*0.8f));
                }
           }
           
            servo.pulsewidth(currDir);
            
            //Start Velocity control after requisite number of encoder signals have been collected
            //if(numInterrupts >= 4){
                //velocity_control(0.1f, TUNING_CONSTANT_10);
            //}        
            
            //Save current direction as previous direction
            prevDir = currDir;
            
            //Prepare to start collecting more data
            integrationCounter = 150;
            
            //Disable interrupts
            //interrupt.fall(NULL);
            //interrupt.rise(NULL);
            
            //Stop timer
            //t.stop();                
        }
        else{
            clk = 1;
            wait_us(intTimMod);
            ADCdata[integrationCounter - 1] = camData;
            clk = 0;
        }

        //clk = 0;
        integrationCounter++;
        if(dataCol){
            loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
        }
        //camData.
        
    }
    if (dataCol){
        //print frame data
        pc.printf("printing frame data\n\r");
        //int frameSize = frames.size();
        //pc.printf("%i",frameSize);
        pc.printf("[");
        for(int i=0; i<numData; i++){
            if(lineCenters > 0){
                pc.printf("%i %i,",lineCenters[i], times[i]);
            }
        }
        pc.printf("]\n\r");
    }
}