File content as of revision 13:f7ec3f026634:

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

//Brightness accommodater


//Data Collection
bool dataCol = true;
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.0011f;
float hardRight = 0.0021f;
//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 = 500;
int lineCenters [numData] = {0};
int times [numData] = {0};
float pSteering [numData] = {0.0f};
float pdSteering [numData] = {0.0f};
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.14f;
int intTimMod = 0;
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// 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 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~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

//PIDcontrol -----------------------------------------------------
int centerSum = 0;
int prevLineCenter = 0;
int prevTime = 0;
int p,i,d = 1;
float timeDiff = 0.0f;
float lastTime = 0.0f;

//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 PIDcontrol(int lineCenter){
    //centerSum += lineCenter;
    float newTime = t.read();
    timeDiff = newTime - lastTime;
    lastTime = newTime; 
    float lineDif = ((float)prevLineCenter - (float)lineCenter)/(float) timeDiff;
    return p * lineCenter + d * lineDif;
    
}

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; 
            //pc.printf("%i", loopCtr); 
            if(loopCtr >= numData){
                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.007f){
                            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.007f){
                        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%6==0){
                        lineCenters[loopCtr] = approxPos;
                        times[loopCtr] = printTimer.read_ms();
                        pSteering[loopCtr] = currDir;
                        pSteering[loopCtr] = PIDcontrol(approxPos);
                        loopCtr++;
                    }
                }
                
                currDir = hardLeft + (approxPos)/((float) 127)*(hardRight-hardLeft);
                prevApproxPos = approxPos;

            }

            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);x
            
            //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 %f %f,",lineCenters[i], times[i], pSteering[i], pdSteering[i]);
            }
        }
        pc.printf("]\n\r");
    }
}