E=MC
/
e=mc
.
Diff: main.cpp
- Revision:
- 14:bda4a189cbe8
- Parent:
- 12:46d0ff953a3f
- Child:
- 15:d746c53bf49b
--- a/main.cpp Fri Mar 20 01:29:47 2015 +0000 +++ b/main.cpp Thu Apr 23 23:29:16 2015 +0000 @@ -1,17 +1,75 @@ #include "mbed.h" - -DigitalOut myled(LED1); +#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); -PwmOut motor1(PTA4); -PwmOut motor2(PTA12); -DigitalOut break1(PTC12); -DigitalOut break2(PTC13); Serial pc(USBTX, USBRX); // tx, rx -// encoder setup and variables +//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; @@ -24,21 +82,27 @@ //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; - -// Servo parameters -float lastTurnTime = 0.0f; -bool servoLeft = true; +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 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; @@ -47,244 +111,264 @@ const float TUNE_AMT = 0.1f; //Parameters specifying sample sizes and delays for small and large average speed samples -float num_samples_small = 10.0f; +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 +//Large and small arrays used to get average velocity values float large_avg_speed_list [100]; float small_avg_speed_list [10]; -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -float get_speed(){ - float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f; - float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE; - return linearSpeed; +//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; + } -float get_avg_speed(float num_samples, float delay) { +int main() { + + //Alter reg values to speed up KL25Z + initADC(); - float avg_avg_speed = 0; + //Line Tracker Initializations + int integrationCounter = 0; + + //Initial values for directions + currDir = 0; + prevDir = 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(); - //pc.printf("\n\rworking: %f", large_avg_speed_list[c]); - } - 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]; - //pc.printf("\n\rworking: %f", large_avg_speed_list[c]); + // 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; } } - return avg_avg_speed/num_samples; -} + 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; -void velocity_control(float duty_cyc, float tuning_const) { - - avg_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 (avg_speed - tuning_const < -1*TUNE_THRESH){ - tuning_val += TUNE_AMT; - stall_check = avg_speed; - } else { - //tuning_val = 1; - stall_check = avg_speed; + } + 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. + } - motor1.pulsewidth(.0025 * duty_cyc * tuning_val); - motor2.pulsewidth(.0025 * duty_cyc * tuning_val); - - pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val); - wait(.2); -} - -void servo_sweep(){ - for(float p = 0.001; p<0.002; p+=0.0001){ - servo.pulsewidth(p); - wait(0.5); + 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"); } } -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; - //pc.printf("dark to light time : %d\n\r", interval); - //pc.printf("fall"); -} -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; - //pc.printf("light to dark time : %d\n\r", interval); - //pc.printf("rise"); -} - -int main() { - servo.period(0.005); - motor1.period(.0025); - motor2.period(.0025); - interrupt.fall(&fallInterrupt); - interrupt.rise(&riseInterrupt); - - t.start(); - while(1){ - - wait(3); - char choice = '1'; //pc.getc(); - pc.putc(choice); - - switch(choice){ - case '0': - motor1.pulsewidth(0.0); - motor2.pulsewidth(0.0); - pc.printf("0% \n\r"); - - break; - case '1': - motor1.pulsewidth(.0025*.1); - motor2.pulsewidth(.0025*.1); - break1 = 0; - break2 = 0; - pc.printf("100% \n\r"); - wait(.5); - pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small)); - - while(1){ - velocity_control(0.1f, TUNING_CONSTANT_10); - } - - //break; - case '2': - motor1.pulsewidth(.0025*.2); - motor2.pulsewidth(.0025*.2); - break1 = 0; - break2 = 0; - pc.printf("\n\r20% \n\r"); - wait(.5); - pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed(num_samples_small, delay_small)); - servoTimer.start(); - while(1){ - - velocity_control(0.2f, TUNING_CONSTANT_20); - if(servoLeft){ - servo.pulsewidth(.001); - } - else{ - servo.pulsewidth(.002); - } - float turnTime = servoTimer.read(); - if(turnTime - lastTurnTime > 3.0){ - servoLeft = !servoLeft; - lastTurnTime = turnTime; - } - } - - //break; - case '3': - motor1.pulsewidth(.0025*.3); - motor2.pulsewidth(.0025*.3); - break1 = 0; - break2 = 0; - pc.printf("\n\r30% \n\r"); - wait(.5); - pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small)); - - while(1){ - velocity_control(0.3f, TUNING_CONSTANT_30); - } - - //break; - case '5': - motor1.pulsewidth(.0025*.5); - motor2.pulsewidth(.0025*.5); - break1 = 0; - break2 = 0; - pc.printf("\n\r50% \n\r"); - wait(.5); - pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small)); - - while(1){ - velocity_control(0.5f, TUNING_CONSTANT_50); - } - - //break; - case 'a': - pc.printf("\n\rGet average velocity of which duty cycle?\n\r"); - choice = pc.getc(); - pc.putc(choice); - - break1 = 0; - break2 = 0; - switch(choice){ - - case '1': - motor1.pulsewidth(.0025*0.1f); - motor2.pulsewidth(.0025*0.1f); - pc.printf("\n\rLongterm average speed at 10 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); - break; - - case '2': - motor1.pulsewidth(.0025*0.2f); - motor2.pulsewidth(.0025*0.2f); - pc.printf("\n\rLongterm average speed at 20 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); - break; - - case '3': - motor1.pulsewidth(.0025*0.3f); - motor2.pulsewidth(.0025*0.3f); - pc.printf("\n\rLongterm average speed at 30 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); - break; - - case '5': - motor1.pulsewidth(.0025*0.5f); - motor2.pulsewidth(.0025*0.5f); - pc.printf("\n\rLongterm average speed at 50 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); - break; - - default: - break; - } - break; - - default: - motor1.pulsewidth(.0025*0); - motor2.pulsewidth(.0025*0); - break1 = 0; - break2 = 0; - pc.printf("default\n\r"); - break; - } - - //servo_sweep(); - } -}