File content as of revision 25:dbadea3c526d:

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

#include "stdlib.h"
#include <vector>

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<float, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
telemetry::Numeric<float> tele_motor_pwm(telemetry_obj, "motor", "Motor PWM", "%DC", 0);

//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 bt(PTA2, PTA1);
//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 = 3;
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;

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Tuning Values that can make it or break it ~~~~~~~~~~~~~~~~~~~~~~~~
float pulsewidth = 0.2f;
int intTimMod = 0;
float maxValThresh = .12; //~.1 for earlier in the day, reduce it (maybe something like .05 - .01 or something) as it gets darker 
bool turnSpeedControl = true; //have increased PWMs when turning when 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

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

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

int main() {
    bluetooth_power.write(1);
    telemetry_serial.baud(115200);
    telemetry_obj.transmit_header();
    
    //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();
    }
    //uint16_t* data = camData.read();
    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);
            tele_time_ms = t.read_ms();
            for (uint16_t i=0; i<128; i++) {
                tele_linescan[i] = ADCdata[i];
            }
            telemetry_obj.do_io();
            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] < maxValThresh){
                            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] < maxValThresh){
                        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*2.0f));
                        motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f));
                } else if (approxPos > 45 && approxPos <= 55){
                        motor1.pulsewidth(motorPeriod*pulsewidth*1.5f);
                        motor2.pulsewidth(motorPeriod*pulsewidth*1.5f);
                } else if (approxPos > 55 && approxPos <= 85){
                        motor1.pulsewidth(motorPeriod*pulsewidth);
                        motor2.pulsewidth(motorPeriod*pulsewidth);
                } else if (approxPos > 85 && approxPos <= 95){
                        motor1.pulsewidth(motorPeriod*pulsewidth*1.5f);
                        motor2.pulsewidth(motorPeriod*pulsewidth*1.5f);
                } else {
                    motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f));
                    motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f));
                }
           }
           
            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");
    }
    */
}