File content as of revision 10:e40ad924e935:

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

//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 maxVal;
int maxLoc;

//Line Crossing variables
int prevTrackLoc;
int spaceThresh = 5;
bool space;

//Servo turning parameters
float straight = 0.00155f;
float hardLeft = 0.0010f;
float hardRight = 0.0021f;

//Servo Directions
float currDir;
float prevDir;

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

float pulsewidth = 0.25f;

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

Timer t;
Timer servoTimer;

//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);
    
    wait(3);

    motor1.pulsewidth(motorPeriod*pulsewidth);
    motor2.pulsewidth(motorPeriod*pulsewidth);
    break1 = 0;
    break2 = 0;
    
    while(1) {
            
        if(integrationCounter % 151== 0){            
            //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);
            
            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.07f && ADCdata[c] > 0.1f){
                    maxAccum += c;
                    maxCount++;
                    if (c > prevTrackLoc + spaceThresh){
                        space = true;
                    }
                    prevTrackLoc = c;
                }
            }
            
            //Line Crossing Checks
            if (maxCount >= 15 || space) {
                currDir = prevDir;
            } else {
                approxPos = (float)maxAccum/(float)maxCount;
                //approxPos = find_track(ADCdata);
                currDir = hardLeft + approxPos/((float) 127)*(hardRight-hardLeft);
            }
            
            //Change speed when turning at different angles
            /*if(approxPos > 0 && approxPos <= 45){
                    motor1.pulsewidth(motorPeriod*(pulsewidth/2));
                    motor2.pulsewidth(motorPeriod*(pulsewidth/2));
            } else if (approxPos > 45 && approxPos <= 95){
                    motor1.pulsewidth(motorPeriod*pulsewidth);
                    motor2.pulsewidth(motorPeriod*pulsewidth);
            } else {
                motor1.pulsewidth(motorPeriod*(pulsewidth/2));
                motor2.pulsewidth(motorPeriod*(pulsewidth/2));
            }*/
            
            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(10);
            ADCdata[integrationCounter - 1] = camData;
            clk = 0;
        }

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