Framework for doing the figure 8 track.
Dependencies: mbed
Fork of Shitty_figure8 by
main.cpp
00001 #include "mbed.h" 00002 #include "stdlib.h" 00003 #include <vector> 00004 //Outputs 00005 DigitalOut led1(LED1); 00006 DigitalOut clk(PTD5); 00007 DigitalOut si(PTD4); 00008 PwmOut motor1(PTA12); 00009 PwmOut motor2(PTA4); 00010 DigitalOut break1(PTC7); 00011 DigitalOut break2(PTC0); 00012 PwmOut servo(PTA5); 00013 00014 Serial pc(USBTX, USBRX); // tx, rx 00015 00016 //Inputs 00017 AnalogIn camData(PTC2); 00018 00019 //Encoder setup and variables 00020 InterruptIn interrupt(PTA13); 00021 00022 //Line Tracking Variables -------------------------------- 00023 float ADCdata [128]; 00024 float maxAccum; 00025 float maxCount; 00026 float approxPos; 00027 float maxVal; 00028 int maxLoc; 00029 00030 //Line Crossing variables 00031 int prevTrackLoc; 00032 int spaceThresh = 5; 00033 bool space; 00034 00035 //Servo turning parameters 00036 float straight = 0.00155f; 00037 float hardLeft = 0.0010f; 00038 float hardRight = 0.0021f; 00039 00040 //Servo Directions 00041 float currDir; 00042 float prevDir; 00043 00044 // All linescan data for the period the car is running on. To be printed after a set amount of time 00045 //std::vector<std::vector<int> > frames; 00046 const int numData = 1000; 00047 int lineCenters [numData] = {0}; 00048 int times [numData] = {0}; 00049 int loopCtr = 0; 00050 00051 //End of Line Tracking Variables ------------------------- 00052 00053 //Encoder and Motor Driver Variables --------------------- 00054 00055 //Intervals used during encoder data collection to measure velocity 00056 int interval1=0; 00057 int interval2=0; 00058 int interval3=0; 00059 int avg_interval=0; 00060 int lastchange1 = 0; 00061 int lastchange2 = 0; 00062 int lastchange3 = 0; 00063 int lastchange4 = 0; 00064 00065 //Variables used to for velocity control 00066 float avg_speed = 0; 00067 float last_speed = 0; 00068 00069 float stall_check = 0; 00070 float tuning_val = 1; 00071 00072 int numInterrupts = 0; 00073 00074 float pulsewidth = 0.15f; 00075 00076 // Hardware periods 00077 float motorPeriod = .0025; 00078 float servoPeriod = .0025; 00079 00080 Timer t; 00081 Timer servoTimer; 00082 Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames 00083 const float printTime = 5.0f; 00084 00085 //Observed average speeds for each duty cycle 00086 const float DESIRED_SPEED = 1; 00087 const float TUNING_CONSTANT_10 = 1.90; 00088 const float TUNING_CONSTANT_20 = 3.00; 00089 const float TUNING_CONSTANT_30 = 4.30; 00090 const float TUNING_CONSTANT_50 = 6.880; 00091 const float PI = 3.14159; 00092 const float WHEEL_CIRCUMFERENCE = .05*PI; 00093 00094 //Velocity Control Tuning Constants 00095 const float TUNE_THRESH = 0.5f; 00096 const float TUNE_AMT = 0.1f; 00097 00098 //Parameters specifying sample sizes and delays for small and large average speed samples 00099 float num_samples_small = 3.0f; 00100 float delay_small = 0.05f; 00101 float num_samples_large = 100.0f; 00102 float delay_large = 0.1f; 00103 00104 //Large and small arrays used to get average velocity values 00105 float large_avg_speed_list [100]; 00106 float small_avg_speed_list [10]; 00107 00108 //End of Encoder and Motor Driver Variables ---------------------- 00109 00110 //Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 00111 00112 int find_track(float line[]){ 00113 int track_location = -1; 00114 float slope_threshold = .05; 00115 bool downslope_indices [128] = {false}; 00116 bool upslope_indices [128] = {false}; 00117 for(int i=10; i<118; i++){ 00118 if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){ 00119 downslope_indices[i] = true; 00120 } 00121 if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){ 00122 upslope_indices[i] = true; 00123 } 00124 } 00125 int numDownslopes = 0; 00126 int numUpslopes = 0; 00127 for(int i=0; i<128; i++){ 00128 if(downslope_indices[i] == true){ 00129 numDownslopes ++; 00130 } 00131 if(upslope_indices[i] == true){ 00132 numUpslopes ++; 00133 } 00134 } 00135 int downslope_locs [numDownslopes]; 00136 int upslope_locs [numUpslopes]; 00137 int dsctr = 0; 00138 int usctr = 0; 00139 00140 for (int i=0; i<128; i++){ 00141 if(downslope_indices[i] == true){ 00142 downslope_locs[dsctr] = i; 00143 dsctr++; 00144 } 00145 if(upslope_indices[i] == true){ 00146 upslope_locs[usctr] = i; 00147 usctr++; 00148 } 00149 } 00150 00151 for(int i=0; i<numDownslopes; i++){ 00152 for(int j=0; j<numUpslopes; j++){ 00153 if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){ 00154 track_location = downslope_locs[i] + 2 ; 00155 } 00156 } 00157 } 00158 00159 return track_location; 00160 } 00161 00162 //Function for speeding up KL25Z ADC 00163 void initADC(void){ 00164 00165 ADC0->CFG1 = ADC0->CFG1 & ( 00166 ~( 00167 0x80 // LDLPC = 0 ; no low-power mode 00168 | 0x60 // ADIV = 1 00169 | 0x10 // Sample time short 00170 | 0x03 // input clock = BUS CLK 00171 ) 00172 ) ; // clkdiv <= 1 00173 ADC0->CFG2 = ADC0->CFG2 00174 | 0x03 ; // Logsample Time 11 = 2 extra ADCK 00175 ADC0->SC3 = ADC0->SC3 00176 & (~(0x03)) ; // hardware avarage off 00177 } 00178 00179 00180 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 00181 00182 float get_speed(){ 00183 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f; 00184 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE; 00185 return linearSpeed; 00186 } 00187 00188 float get_avg_speed(float num_samples, float delay) { 00189 00190 float avg_avg_speed = 0; 00191 00192 for (int c = 0; c < num_samples; c++) { 00193 if (num_samples == num_samples_small){ 00194 small_avg_speed_list[c] = get_speed(); 00195 } else if (num_samples == num_samples_large){ 00196 large_avg_speed_list[c] = get_speed(); 00197 } 00198 //wait(delay); 00199 } 00200 00201 for (int c = 1; c < num_samples; c++) { 00202 if (num_samples == num_samples_small){ 00203 avg_avg_speed += small_avg_speed_list[c]; 00204 } else if (num_samples == num_samples_large){ 00205 avg_avg_speed += large_avg_speed_list[c]; 00206 } 00207 } 00208 return avg_avg_speed/num_samples; 00209 } 00210 00211 void velocity_control(float duty_cyc, float tuning_const) { 00212 00213 avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small); 00214 00215 //When determined speed is infinity or 0, set the speed to the last agreeable speed 00216 /*if (avg_speed > 100 || avg_speed == 0){ 00217 avg_speed = last_speed; 00218 }*/ 00219 00220 pc.printf("\n\r%f", avg_speed); 00221 if (avg_speed == stall_check && tuning_const != 0 && avg_speed == 0) { 00222 avg_speed = 0; 00223 tuning_val += TUNE_AMT; 00224 } else if((avg_speed - tuning_const) > TUNE_THRESH){ 00225 tuning_val -= TUNE_AMT; 00226 stall_check = avg_speed; 00227 00228 } else if (tuning_const - avg_speed > TUNE_THRESH && avg_speed != 0){ 00229 tuning_val += TUNE_AMT; 00230 stall_check = avg_speed; 00231 } else { 00232 //tuning_val = 1; 00233 stall_check = avg_speed; 00234 } 00235 00236 if (tuning_val < .5){ 00237 tuning_val = .5; 00238 } 00239 pc.printf("\n\rTuning Val: %f", tuning_val); 00240 motor1.pulsewidth(motorPeriod * (duty_cyc * tuning_val)); 00241 motor2.pulsewidth(motorPeriod * (duty_cyc * tuning_val)); 00242 00243 if (avg_speed != 0){ 00244 last_speed = avg_speed; 00245 } 00246 00247 } 00248 00249 // Interrupt Functions for Encoder ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~`` 00250 void fallInterrupt(){ 00251 00252 int time = t.read_us(); 00253 interval1 = time - lastchange2; 00254 interval2 = lastchange1-lastchange3; 00255 interval3 = lastchange2 - lastchange4; 00256 avg_interval = (interval1 + interval2 + interval3)/3; 00257 00258 lastchange4 = lastchange3; 00259 lastchange3 = lastchange2; 00260 lastchange2 = lastchange1; 00261 lastchange1 = time; 00262 00263 numInterrupts++; 00264 } 00265 00266 void riseInterrupt(){ 00267 int time = t.read_us(); 00268 interval1 = time - lastchange2; 00269 interval2 = lastchange1-lastchange3; 00270 interval3 = lastchange2 - lastchange4; 00271 avg_interval = (interval1 + interval2 + interval3)/3; 00272 00273 lastchange4 = lastchange3; 00274 lastchange3 = lastchange2; 00275 lastchange2 = lastchange1; 00276 lastchange1 = time; 00277 00278 numInterrupts++; 00279 } 00280 00281 00282 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 00283 00284 int main() { 00285 00286 //Alter reg values to speed up KL25Z 00287 initADC(); 00288 00289 //Line Tracker Initializations 00290 int integrationCounter = 0; 00291 00292 //Initial values for directions 00293 currDir = 0; 00294 prevDir = 0; 00295 00296 // Motor Driver Initializations 00297 motor1.period(motorPeriod); 00298 motor2.period(motorPeriod); 00299 00300 // Servo Initialization 00301 servo.period(servoPeriod); 00302 00303 wait(3); 00304 00305 motor1.pulsewidth(motorPeriod*pulsewidth); 00306 motor2.pulsewidth(motorPeriod*pulsewidth); 00307 break1 = 0; 00308 break2 = 0; 00309 00310 int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0; 00311 t.start(); 00312 printTimer.start(); 00313 //wait(5); 00314 00315 while(1) { 00316 //break out of main loop if enough time has passed; 00317 if(loopCtr >= numData){ 00318 break; 00319 } 00320 if(integrationCounter % 151== 0){ 00321 //Disable interrupts 00322 interrupt.fall(NULL); 00323 interrupt.rise(NULL); 00324 00325 /* 00326 // copy frame into the vector to be printed 00327 std::vector<int> temp; 00328 temp.reserve(128); //performance 00329 for(int i=0; i<128; i++){ 00330 temp.push_back(ADCdata[i]); 00331 } 00332 frames.push_back(temp); 00333 */ 00334 00335 //Send start of integration signal 00336 si = 1; 00337 clk = 1; 00338 00339 si = 0; 00340 clk = 0; 00341 00342 //Reset timing counter for integration 00343 integrationCounter = 0; 00344 00345 //Reset line tracking variables 00346 maxAccum = 0; 00347 maxCount = 0; 00348 approxPos = 0; 00349 00350 space = false; 00351 00352 } 00353 else if (integrationCounter > 129){ 00354 //Start Timer 00355 //t.start(); 00356 00357 //Enable interrupts 00358 //interrupt.fall(&fallInterrupt); 00359 //interrupt.rise(&riseInterrupt); 00360 00361 maxVal = ADCdata[10]; 00362 for (int c = 11; c < 118; c++) { 00363 if (ADCdata[c] > maxVal){ 00364 maxVal = ADCdata[c]; 00365 maxLoc = c; 00366 } 00367 } 00368 00369 for (int c = 10; c < 118; c++) { 00370 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){ 00371 maxAccum += c; 00372 maxCount++; 00373 if (c > prevTrackLoc + spaceThresh){ 00374 space = true; 00375 } 00376 prevTrackLoc = c; 00377 } 00378 } 00379 00380 //Line Crossing Checks 00381 if (maxCount >= 15 || space) { 00382 currDir = prevDir; 00383 } else { 00384 approxPos = (float)maxAccum/(float)maxCount; 00385 00386 if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){ 00387 lineCenters[loopCtr] = approxPos; 00388 times[loopCtr] = printTimer.read_ms(); 00389 loopCtr++; 00390 } 00391 currDir = hardLeft + approxPos/((float) 127)*(hardRight-hardLeft); 00392 00393 00394 } 00395 00396 //Change speed when turning at different angles 00397 /*if(approxPos > 0 && approxPos <= 45){ 00398 motor1.pulsewidth(motorPeriod*(pulsewidth/2)); 00399 motor2.pulsewidth(motorPeriod*(pulsewidth/2)); 00400 } else if (approxPos > 45 && approxPos <= 95){ 00401 motor1.pulsewidth(motorPeriod*pulsewidth); 00402 motor2.pulsewidth(motorPeriod*pulsewidth); 00403 } else { 00404 motor1.pulsewidth(motorPeriod*(pulsewidth/2)); 00405 motor2.pulsewidth(motorPeriod*(pulsewidth/2)); 00406 }*/ 00407 00408 servo.pulsewidth(currDir); 00409 00410 //Start Velocity control after requisite number of encoder signals have been collected 00411 //if(numInterrupts >= 4){ 00412 //velocity_control(0.1f, TUNING_CONSTANT_10); 00413 //} 00414 00415 //Save current direction as previous direction 00416 prevDir = currDir; 00417 00418 //Prepare to start collecting more data 00419 integrationCounter = 150; 00420 00421 //Disable interrupts 00422 //interrupt.fall(NULL); 00423 //interrupt.rise(NULL); 00424 00425 //Stop timer 00426 //t.stop(); 00427 } 00428 else{ 00429 clk = 1; 00430 wait_us(10); 00431 ADCdata[integrationCounter - 1] = camData; 00432 clk = 0; 00433 } 00434 00435 //clk = 0; 00436 integrationCounter++; 00437 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++; 00438 //camData. 00439 00440 } 00441 00442 //print frame data 00443 pc.printf("printing frame data\n\r"); 00444 //int frameSize = frames.size(); 00445 //pc.printf("%i",frameSize); 00446 pc.printf("["); 00447 for(int i=0; i<numData; i++){ 00448 if(lineCenters > 0){ 00449 pc.printf("%i %i,",lineCenters[i], times[i]); 00450 } 00451 } 00452 pc.printf("]\n\r"); 00453 } 00454
Generated on Fri Jul 22 2022 00:36:08 by
1.7.2
