E=MC
/
e=mc
.
main.cpp
- Committer:
- cheryl_he
- Date:
- 2015-04-24
- Revision:
- 17:054a54d73e04
- Parent:
- 16:f7ab57048e1e
- Child:
- 18:9508bece39b0
File content as of revision 17:054a54d73e04:
#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}; float lineDifData [numData] = {0.0f}; float pdLineCenter [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; float prevLineCenter = 0.0f; int prevTime = 0; float p,i,d = 1.0f; 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, int loopCtr){ //centerSum += lineCenter; float newLineCenter = (float)lineCenter/127.0f; float newTime = t.read(); timeDiff = newTime - lastTime; lastTime = newTime; float lineDif = ((float)newLineCenter - (float)prevLineCenter)/(float) timeDiff; lineDifData[loopCtr] = lineDif; if(prevLineCenter != newLineCenter){ prevLineCenter = newLineCenter; } return p * newLineCenter + 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(dataCol){ if (space) { currDir = prevDir; if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%8==0){ lineCenters[loopCtr] = approxPos; times[loopCtr] = printTimer.read_ms(); pSteering[loopCtr] = currDir; pdSteering[loopCtr] = PIDcontrol(approxPos, loopCtr); loopCtr++; } firstTime = true; } else { approxPos = (float)maxAccum/(float)maxCount; currDir = hardLeft + (approxPos)/((float) 127)*(hardRight-hardLeft); if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%8==0){ lineCenters[loopCtr] = approxPos; times[loopCtr] = printTimer.read_ms(); pSteering[loopCtr] = currDir; pdSteering[loopCtr] = PIDcontrol(approxPos, loopCtr); loopCtr++; } 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); for(int i=0; i<numData; i+=10){ if(lineCenters > 0){ /*pc.printf("LINE #: %i\n\r",i); pc.printf("line center: %i\n\r", lineCenters[i]); pc.printf("time: %i\n\r", times[i]); pc.printf("pSteer: %f\n\r", pSteering[i]); pc.printf("pdSteer: %f\n\r", pdSteering[i]); pc.printf("lineDif: %f\n\r", lineDifData[i]); pc.printf("pdLineCenter: %f\n\r", pdLineCenter[i]); */ pc.printf("%i %i %f %f %f %f\n\r",lineCenters[i], times[i], pSteering[i], pdSteering[i], lineDifData[i], pdLineCenter[i]); } } } }