File content as of revision 0:ad375c052b4c:

#include "mbed.h"
#include "stdlib.h"

//Outputs
DigitalOut led1(LED1);
DigitalOut clk(PTD5);
DigitalOut si(PTD4);
PwmOut motor1(PTA4);
PwmOut motor2(PTA12);
DigitalOut break1(PTC12);
DigitalOut break2(PTC13);
PwmOut servo(PTA5);

//Inputs
AnalogIn camData(PTC2);

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

//Line Tracking Variables --------------------------------
float ADCdata [128];
float minAccum;
float minCount;
float approxPos;
float minVal;
int minLoc;

//Servo turning parameters
float straight = 0.00155f;
float hardLeft = 0.0013f;
float slightLeft = 0.00145f;
float hardRight = 0.0018f;
float slightRight = 0.00165f;

//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 stall_check = 0;
float tuning_val = 1;

Timer t;
Timer servoTimer;

//Observed average speeds for each duty cycle
const float DESIRED_SPEED = 0.5;
const float TUNING_CONSTANT_10 = 1.10;
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 ----------------------

// 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);
    
    if (avg_speed == stall_check) {
        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){
        //tuning_val += TUNE_AMT;
        stall_check = avg_speed;
    } else {
        //tuning_val = 1;
        stall_check = avg_speed;
    }
    motor1.pulsewidth(.0025 * duty_cyc * tuning_val);
    motor2.pulsewidth(.0025 * duty_cyc * tuning_val);
    
}

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

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


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

int main() {
    //Line Tracker Initializations
    int integrationCounter = 0;
    
    // Motor Driver Initializations
    motor1.period(.0025);
    motor2.period(.0025);
    interrupt.fall(&fallInterrupt);
    interrupt.rise(&riseInterrupt);

    //wait(5);

    motor1.pulsewidth(.0025*.1);
    motor2.pulsewidth(.0025*.1);
    break1 = 0;
    break2 = 0;
    
    t.start();
    
    wait(5);
    
    while(1) {
            
        if(integrationCounter % 151== 0){
            //Disable interrupts
            //__disable_irq();
            //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
            minAccum = 0;
            minCount = 0;
            approxPos = 0;
            
            //Reset Encoder and Motor Driver Variables
            interval1=0;
            interval2=0;
            interval3=0;
            avg_interval=0;
            lastchange1 = 0;
            lastchange2 = 0;
            lastchange3 = 0;
            lastchange4 = 0;
            
            t.reset();
                
        }
        else if (integrationCounter > 129){
            //Enable interrupts
            //__enable_irq();
            //interrupt.fall(&fallInterrupt);
            //interrupt.rise(&riseInterrupt);
            
            minVal = ADCdata[15];
            for (int c = 15; c < 118; c++) {
                if (ADCdata[c] < minVal){
                    minVal = ADCdata[c];
                    minLoc = c;
                }
            }
            
            for (int c = 15; c < 118; c++) {
                if (ADCdata[c] >= minVal && ADCdata[c] - minVal < 0.04f && ADCdata[c] > 0.1f){
                    minAccum += c;
                    minCount++;
                }
            }
            
            approxPos = (float)minAccum/(float)minCount;
            
            if(approxPos > 0 && approxPos <= 20){
                    servo.pulsewidth(hardLeft);
            } else if (approxPos > 20 && approxPos <= 45){
                    servo.pulsewidth(slightLeft);
            } else if (approxPos > 45 && approxPos <= 90){
                servo.pulsewidth(straight);
            } else if (approxPos > 90 && approxPos <= 105){
                servo.pulsewidth(slightRight);
            } else if (approxPos > 105 && approxPos <= 128){
                    servo.pulsewidth(hardRight);
            }
            
            velocity_control(0.05f, DESIRED_SPEED);
            
            integrationCounter = 150;
        }
        else{
            clk = 1;
            wait_us(70);
            ADCdata[integrationCounter - 1] = camData;
            clk = 0;
        }

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