Daniel Hadad
This is an example program that actually allows the car to race using the FRDM-TFC library!
Fork of
TFC-RACING-DEMO
by 
Daniel Hadad
Embed:
(wiki syntax)
Show/hide line numbers
Spices.cpp
00001 #include "mbed.h" 00002 00003 #include "Spices.h" 00004 #include "common.h" 00005 #include "TFC.h" 00006 00007 // camera params 00008 #define NUM_LINE_SCAN 128 00009 #define MAX_LINE_SCAN NUM_LINE_SCAN-1 00010 #define MIN_LINE_WIDTH 0 00011 #define MAX_LINE_WIDTH 15 00012 #define FILTER_ENDS 0 // # of pixels at end of camera data to ignore; set to 0 for now, later make 15 00013 #define RANGE (NUM_LINE_SCAN - (2 * FILTER_ENDS)) // range of camera pixels to consider 00014 #define ERR_RATIO 0.85 // ratio of max possible error to pixels (have to measure!) 00015 #define DER_RATIO 0.5 // ratio for der threshold level (was 0.5 initially, may put back) 00016 00017 // steer/servo params 00018 #define MAX_STEER_LEFT -0.42 // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach) 00019 #define MAX_STEER_RIGHT 0.43 // value determined by demo mode 1 measure 00020 #define DT 0.02 // # MS of time between intervals (doesn't really matter) 00021 00022 // logging parameters 00023 #define NUM_LOG_FRAMES 700 // # of frames to log (when logging active) ~14 sec worth! 00024 00025 // ****** for debug tuning ****** 00026 #define TUNE_KP 0.008 00027 #define TUNE_KI 0 00028 #define TUNE_KD 0 00029 #define MIN_POWER 60 // percent min power (estimating for a 2-ft turn => 24" / (24" + 6" car) = 4/5; speed of inner wheel is 20% lower worst case 00030 #define SPEED_ADJUST 4 // do not change 00031 #define ABS_ERROR_THRESH 10 // number of pixels line position offset before changing KP value 00032 #define CONTROL_METHOD 1 // which control method to use 00033 00034 00035 // Drive/motor params 00036 #define LIGHT_SPEED 0.4 // easy speed 00037 #define LUDICROUS_SPEED 0.6 // faster speed 00038 #define MAX_POWER 100 // percent max power (for speed adjustments) 00039 00040 // algo params 00041 #define UNKNOWN_COUNT_MAX 50 // max value to allow for unknown track conditions before killing engine 00042 #define STARTGATEFOUNDMAX 0 // max value to allow for finding starting gate before killing engine 00043 #define STARTGATEDELAY 50 // Delay before searching for starting gate to kill engine 00044 00045 00046 // image processing vars 00047 uint16_t GrabLineScanImage0[NUM_LINE_SCAN]; // snapshot of camera data for this 'frame' 00048 float DerivLineScanImage0[NUM_LINE_SCAN]; // derivative of line scan data 00049 float NegEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data negative edges found 00050 float PosEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data positive edges found 00051 uint16_t numNegEdges = 0, numPosEdges = 0; // max value of valid neg and positive indices (also serves as a count of # edges found) 00052 uint16_t MaxLightIntensity = 0; // max measured light intensity -- to account for lighting differences 00053 uint16_t MinLightIntensity = (1 << 12); // min measured light intensity -- to account for lighting differences 00054 float maxDerVal = 0; // max deriv value 00055 float minDerVal = (float) (1 << 12); // min deriv value 00056 float aveDerVal = 0; // average deriv value 00057 float DerivThreshold = (1 << 9); // Derivative Threshold (default) 00058 float PosDerivThreshold = (1 << 9); // Pos Edge Derivative Threshold (default) 00059 float NegDerivThreshold = (1 << 9); // Neg Edge Derivative Threshold (default) 00060 00061 00062 // Steering control variables 00063 float CurrentLinePosition; // Current position of track line (in pixels -- 0 to 127) 00064 float LastLinePosition; // Last position of track line (in pixels -- 0 to 127) 00065 float CurrentLinePosError = 0; // Current line position error (used for derivative calc) 00066 float AbsError; 00067 float LastLinePosError = 0; // Last line position error (used for derivative calc) 00068 float SumLinePosError = 0; // Sum of line position error (used for integral calc) 00069 float DerivError = 0; // Derivative of error 00070 float CurrentSteerSetting = (MAX_STEER_RIGHT + MAX_STEER_LEFT) / 2; // drive straight at first 00071 float CurrentLeftDriveSetting = 0; // Drive setting (left wheel) 00072 float CurrentRightDriveSetting = 0; // Drive setting (right wheel) 00073 00074 // Speed control vars 00075 float MaxSpeed; // maximum speed allowed 00076 00077 uint16_t startRaceTicker; // ticker at start of race1 00078 00079 // Custom Data Types 00080 typedef enum TrackStatusType {Unknown, 00081 LineFound, 00082 StartGateFound, 00083 LineJustLeft} TrackStatusType; 00084 00085 TrackStatusType CurrentTrackStatus; // current track status 00086 TrackStatusType LastTrackStatus; // last track status 00087 00088 /* typedef enum TrackType {NotSure, 00089 Straight, 00090 Curve, 00091 Wiggle, 00092 Bumps, 00093 StartGate, 00094 UpHill, 00095 DownHill} TrackType; 00096 00097 TrackType CurrentTrack; */ 00098 00099 00100 struct LogData { 00101 float linepos; 00102 float steersetting; 00103 float leftdrivesetting; 00104 float rightdrivesetting; 00105 }; 00106 00107 LogData frameLogs[NUM_LOG_FRAMES]; // array of log data to store 00108 int logDataIndex; // index for log data 00109 00110 00111 int StartGateFoundCount = 0; // how many times start gate has been found 00112 int UnknownCount = 0; // how many times nothing has been found (to help with kill switch implementation) 00113 bool go = false; // Car can go! Should be set to false to start. 00114 00115 // EXTRA CONTROL PARAMETERS 00116 bool debugFakeMode = false; // if true, ignores real camera and uses fake camera input instead; used for data processing debug 00117 bool terminalOutput = false; // whether output terminal data 00118 bool doLogData = false; // whether to capture log data to output later on 00119 bool killSwitch = false; // whether to enable Kill Switch (allow engine to stop after not finding track) 00120 bool startGateStop = false; // whether to stop or not depending on starting gate reading 00121 bool doRisky = false; // race style-- whether conservative or risky 00122 00123 void TrackMode() 00124 { 00125 // set mode based on DIP switches 00126 useMode(); 00127 00128 // grab Terminal Mode based on DIP switch 4 00129 terminalOutput = terminalMode(); 00130 00131 // Every 2s (or Terminal Output is off, i.e. race mode!) 00132 // AND line scan image ready (or fake mode where image is always ready) 00133 // (ticker updates every 10ms) (Line scan image ready every 10ms) 00134 if((TFC_Ticker[0]>2000 || (!terminalOutput)) && (TFC_LineScanImageReady>0 || debugFakeMode)) 00135 { 00136 00137 // stuff that needs to be reset with each image frame 00138 TFC_Ticker[0] = 0; 00139 TFC_LineScanImageReady=0; // must reset to 0 after detecting non-zero 00140 MaxLightIntensity = 0; // reset 00141 MinLightIntensity = (1 << 12); // reset 00142 00143 // grab camera frame 00144 grabCameraFrame(); 00145 00146 // calcalate derivative of linescandata, filter starttime data 00147 derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]); 00148 00149 00150 // adjust deriv threshold based on max lighting value 00151 // has to be called before find edges 00152 adjustLights(); 00153 00154 //find edges from derivative data 00155 findEdges_v2(&DerivLineScanImage0[0]); 00156 00157 //review edge data and set position or starting flag appropriately 00158 reviewEdges(); 00159 00160 // print terminal data if desired (slows down things) 00161 if (terminalOutput) { 00162 printLineScanData(&GrabLineScanImage0[0]); 00163 printDerivLineScanData(&DerivLineScanImage0[0]); 00164 printAdjustLightsData(); 00165 printEdgesFound(); 00166 } 00167 00168 // ** Track Status available at this point ** 00169 00170 // Update things based on latest track status 00171 // e.g. change steering setting, stop car, ... 00172 ActOnTrackStatus(); 00173 00174 //give LED feedback as to track status 00175 feedbackLights(); 00176 00177 // control max power (speed) 00178 SpeedControl(); 00179 00180 // Drive!! 00181 Drive(); 00182 00183 if (terminalOutput) { 00184 TERMINAL_PRINTF("\r\n**************************END********************************\r\n"); 00185 } 00186 00187 } 00188 } 00189 00190 uint16_t getMode(){ 00191 // get mode based on first 3 DIP switches (1, 2, 3) 00192 return (TFC_GetDIP_Switch()&0x07); 00193 } 00194 00195 void useMode() 00196 { 00197 00198 switch(getMode()) 00199 { 00200 default: 00201 case 4 : // Track Mode, Auto Steer, safe settings 00202 doRisky = false; 00203 break; 00204 00205 case 5 : // Track Mode, Auto Steer, fast settings 00206 doRisky = true; 00207 break; 00208 00209 case 6 : 00210 // NOT USED YET 00211 break; 00212 00213 case 7 : 00214 // NOT USED YET 00215 break; 00216 00217 } // end case 00218 00219 if(TFC_GetDIP_Switch()&0x08) // SWITCH 4 Terminal Output on/off 00220 startGateStop = true; 00221 else 00222 startGateStop = false; 00223 00224 } 00225 00226 bool terminalMode() 00227 { 00228 return ((TFC_GetDIP_Switch()>>3)&0x01 == 1); 00229 } 00230 00231 void grabCameraFrame() 00232 { 00233 uint32_t i = 0; 00234 uint8_t fake_type = 4; // type of fake data if used 00235 00236 for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0 00237 { 00238 00239 if (debugFakeMode) { // use fake camera data 00240 switch (fake_type) { 00241 case 0: // ideal track -- line in center 00242 if (i<57 || i > 70) 00243 GrabLineScanImage0[i] = 0xFFF; // no line 00244 else 00245 GrabLineScanImage0[i] = 0x4B0; // line 00246 break; 00247 case 1: // ideal track -- line to the left 00248 if (i<27 || i > 40) 00249 GrabLineScanImage0[i] = 0xFFF; // no line 00250 else 00251 GrabLineScanImage0[i] = 0x4B0; // line 00252 break; 00253 case 2: // ideal track -- line to the right 00254 if (i<87 || i > 100) 00255 GrabLineScanImage0[i] = 0xFFF; // no line 00256 else 00257 GrabLineScanImage0[i] = 0x4B0; // line 00258 break; 00259 case 3: // ideal track -- starting gate! 00260 // TBD 00261 break; 00262 case 4: // less than ideal track -- debug multi-edge issue! 00263 if (i<54) 00264 GrabLineScanImage0[i] = 4000; // no line 00265 if (i == 54) 00266 GrabLineScanImage0[i] = 3370; // neg edge 00267 if (i == 55) 00268 GrabLineScanImage0[i] = 3309; // neg edge 00269 if (i == 56) 00270 GrabLineScanImage0[i] = 2016; // neg edge 00271 if (i == 57) 00272 GrabLineScanImage0[i] = 711; // neg edge 00273 if (i == 58) 00274 GrabLineScanImage0[i] = 696; // neg edge 00275 if ((i>58) && (i<69)) 00276 GrabLineScanImage0[i] = 500; // line 00277 if (i == 69) 00278 GrabLineScanImage0[i] = 1800; // pos edge 00279 if (i > 69) 00280 GrabLineScanImage0[i] = 4000; // no line 00281 default: 00282 break; 00283 } 00284 00285 } else { // use real camera data 00286 GrabLineScanImage0[i] = TFC_LineScanImage0[i]; 00287 } 00288 } 00289 00290 00291 } 00292 00293 void printLineScanData(uint16_t* LineScanData) 00294 { 00295 uint32_t i = 0; 00296 float Val; 00297 00298 TERMINAL_PRINTF("LINE SCAN DATA:,"); 00299 00300 for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0 00301 { 00302 if (1 == 1) { // use float to print 00303 Val = (float) LineScanData[i]; 00304 TERMINAL_PRINTF("%9.3f",Val); 00305 if(i==MAX_LINE_SCAN) // when last data reached put in line return 00306 TERMINAL_PRINTF("\r\n"); 00307 else 00308 TERMINAL_PRINTF(","); 00309 } else { 00310 TERMINAL_PRINTF("0x%X",LineScanData[i]); 00311 if(i==MAX_LINE_SCAN) // when last data reached put in line return 00312 TERMINAL_PRINTF("\r\n",LineScanData[i]); 00313 else 00314 TERMINAL_PRINTF(",",LineScanData[i]); 00315 } 00316 } 00317 00318 } 00319 00320 void printDerivLineScanData(float* derivLineScanData) 00321 { 00322 uint32_t i, minCnt = 0, maxCnt = 0; 00323 00324 minCnt = FILTER_ENDS; 00325 maxCnt = NUM_LINE_SCAN - FILTER_ENDS; 00326 00327 TERMINAL_PRINTF("DERIVATIVE DATA:,"); 00328 00329 for(i=minCnt;i<maxCnt;i++) // print one line worth of data (128) from Camera 0 00330 { 00331 TERMINAL_PRINTF("%9.3f",derivLineScanData[i]); 00332 if(i==maxCnt-1) // when last data reached put in line return 00333 TERMINAL_PRINTF("\r\n",derivLineScanData[i]); 00334 else 00335 TERMINAL_PRINTF(", ",derivLineScanData[i]); 00336 } 00337 00338 } 00339 00340 void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut) 00341 { 00342 00343 uint32_t i, minCnt = 0, maxCnt = 0; 00344 float DerVal, upperDerVal, lowerDerVal = 0; 00345 00346 maxDerVal = 0; 00347 minDerVal = (float) (1 << 12); 00348 aveDerVal = 0; 00349 00350 minCnt = FILTER_ENDS; 00351 maxCnt = NUM_LINE_SCAN - FILTER_ENDS; 00352 00353 // TERMINAL_PRINTF("i, upperDerVal, lowerDerVal, DerVal\r\n"); 00354 00355 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 00356 { 00357 00358 // store max light intensity value 00359 if (LineScanDataIn[i] > MaxLightIntensity) 00360 MaxLightIntensity = LineScanDataIn[i]; 00361 00362 // store min light intensity value 00363 if (LineScanDataIn[i] < MinLightIntensity) 00364 MinLightIntensity = LineScanDataIn[i]; 00365 00366 00367 // Central Derivative 00368 if (i==minCnt) { // start point 00369 upperDerVal = (float)(LineScanDataIn[i+1]); 00370 lowerDerVal = (float)(LineScanDataIn[i]); // make same as start point 00371 } else if (i==maxCnt - 1){ // end point 00372 upperDerVal = (float)(LineScanDataIn[i]); // make same as end point 00373 lowerDerVal = (float)(LineScanDataIn[i-1]); 00374 } else { // any other point 00375 upperDerVal = (float)(LineScanDataIn[i+1]); 00376 lowerDerVal = (float)(LineScanDataIn[i-1]); 00377 } 00378 DerVal = (upperDerVal - lowerDerVal) / 2; 00379 // TERMINAL_PRINTF("%d,%9.3f,%9.3f,%9.3f\r\n", i, upperDerVal, lowerDerVal, DerVal); 00380 00381 if (DerVal > maxDerVal) { 00382 maxDerVal = DerVal; 00383 } 00384 if (DerVal < minDerVal) { 00385 minDerVal = DerVal; 00386 } 00387 aveDerVal = aveDerVal + DerVal; //get sum 00388 DerivLineScanDataOut[i] = DerVal; 00389 } 00390 aveDerVal = (float) aveDerVal / (maxCnt - minCnt); 00391 } 00392 00393 //Not reliable for finding edges! 00394 void findEdges(float* derivLineScanData) 00395 { 00396 // search for edges in deriviative data using a threshold 00397 // need to store in a hash if that's possible... 00398 // combine edges that are a pixel apart 00399 00400 int i, minCnt = 0, maxCnt = 0; 00401 int multiNegEdgeCnt = 1, multiNegEdgeSum = 0; 00402 int multiPosEdgeCnt = 1, multiPosEdgeSum = 0; 00403 00404 minCnt = FILTER_ENDS; 00405 maxCnt = NUM_LINE_SCAN - FILTER_ENDS; 00406 00407 numNegEdges = 0; 00408 numPosEdges = 0; 00409 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 00410 { 00411 if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND! 00412 { 00413 if (terminalOutput) { 00414 TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); 00415 } 00416 00417 if ((numNegEdges > 0) && (NegEdges[numNegEdges - 1] + 1 == i )) // if no multi edges 00418 { // edge actually across multiple pixels 00419 multiNegEdgeCnt++; 00420 multiNegEdgeSum = multiNegEdgeSum + i; 00421 if (terminalOutput) { 00422 TERMINAL_PRINTF("MULTIEDGE FOUND! MultiNegEdgeCnt: %d; MultiNegEdgeSum: %d\r\n", multiNegEdgeCnt, multiNegEdgeSum); 00423 } 00424 } else { // not a multi-pixel edge known at this time, store negative edge index value 00425 numNegEdges++; 00426 if (terminalOutput) { 00427 TERMINAL_PRINTF("NEG EDGE STORED WITH INDEX %d. NUM NEG EDGES = %d\r\n", i, numNegEdges); 00428 } 00429 NegEdges[numNegEdges - 1] = (float) i; 00430 multiNegEdgeSum = i; 00431 } 00432 00433 00434 } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND! 00435 00436 if (terminalOutput) { 00437 TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); 00438 } 00439 00440 if ((numPosEdges > 0) && (PosEdges[numPosEdges - 1] + 1 == i )) 00441 { // edge actually across multiple pixels 00442 multiPosEdgeCnt++; 00443 multiPosEdgeSum = multiPosEdgeSum + i; 00444 if (terminalOutput) { 00445 TERMINAL_PRINTF("MULTIEDGE FOUND! MultiPosEdgeCnt: %d; MultiPosEdgeSum: %d\r\n", multiPosEdgeCnt, multiPosEdgeSum); 00446 } 00447 } else { // not a multi-pixel edge known at this time, store Posative edge index value 00448 if (terminalOutput) { 00449 TERMINAL_PRINTF("POS EDGE STORED WITH INDEX %d. NUM POS EDGES = %d\r\n", i, numPosEdges); 00450 } 00451 numPosEdges++; 00452 PosEdges[numPosEdges - 1] = (float) i; 00453 multiPosEdgeSum = i; 00454 } 00455 00456 } else { // NO EDGE FOUND 00457 // combine multi-edges if there are any 00458 if (multiNegEdgeCnt > 1) 00459 { 00460 NegEdges[numNegEdges - 1] = (float) multiNegEdgeSum / multiNegEdgeCnt; 00461 multiNegEdgeCnt = 1; multiNegEdgeSum = 0; 00462 } 00463 if (multiPosEdgeCnt > 1) 00464 { 00465 PosEdges[numPosEdges - 1] = (float) multiPosEdgeSum / multiPosEdgeCnt; 00466 multiPosEdgeCnt = 1; multiPosEdgeSum = 0; 00467 } 00468 00469 } 00470 } 00471 00472 } 00473 00474 00475 void findEdges_v2(float* derivLineScanData) 00476 { 00477 // search for edges in deriviative data using a threshold 00478 // need to store in a hash if that's possible... 00479 // combine edges that are a pixel apart 00480 00481 int i; 00482 00483 int NegEdgeBufCnt = 0, NegEdgeBufSum = 0; // serves as buffer to store neg edges found next to each other 00484 int PosEdgeBufCnt = 0, PosEdgeBufSum = 0; // serves as buffer to store pos edges found next to each other 00485 00486 int minCnt = FILTER_ENDS; 00487 int maxCnt = NUM_LINE_SCAN - FILTER_ENDS; 00488 00489 00490 00491 numNegEdges = 0; // count of neg edges found thus far 00492 numPosEdges = 0; // count of pos edges found thus far 00493 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 00494 { 00495 00496 if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND! 00497 { 00498 00499 if (terminalOutput) { 00500 TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); 00501 } 00502 00503 NegEdgeBufCnt++; // add value to neg edge buffer 00504 NegEdgeBufSum = NegEdgeBufSum + i; 00505 00506 } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND! 00507 00508 if (terminalOutput) { 00509 TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); 00510 } 00511 00512 PosEdgeBufCnt++; // add value to pos edge buffer 00513 PosEdgeBufSum = PosEdgeBufSum + i; 00514 00515 } else { // NO EDGE FOUND 00516 00517 // POP EDGE BUFFERS IF NON-EMPTY AND STORE TO EDGE "STACK" (i.e. edges found) 00518 00519 if (NegEdgeBufCnt > 0) { 00520 // store edge value 00521 numNegEdges++; 00522 NegEdges[numNegEdges - 1] = (float) NegEdgeBufSum / NegEdgeBufCnt; 00523 00524 // clear edge buffer 00525 NegEdgeBufSum = 0; NegEdgeBufCnt = 0; 00526 } 00527 00528 if (PosEdgeBufCnt > 0) { 00529 // store edge value 00530 numPosEdges++; 00531 PosEdges[numPosEdges - 1] = (float) PosEdgeBufSum / PosEdgeBufCnt; 00532 00533 // clear edge buffer 00534 PosEdgeBufSum = 0; PosEdgeBufCnt = 0; 00535 } 00536 00537 } 00538 00539 } 00540 00541 } 00542 00543 void printEdgesFound() 00544 { 00545 int i; 00546 00547 // Check that neg edges captured ok 00548 TERMINAL_PRINTF("NEGATIVE EDGES FOUND:,"); 00549 for(i=0;i<=numNegEdges-1;i++) 00550 { 00551 TERMINAL_PRINTF("%9.3f",NegEdges[i]); 00552 if(i==numNegEdges-1) // when last data reached put in line return 00553 TERMINAL_PRINTF("\r\n"); 00554 else 00555 TERMINAL_PRINTF(", "); 00556 } 00557 00558 00559 // Check that pos edges captured ok 00560 TERMINAL_PRINTF("POSITIVE EDGES FOUND:,"); 00561 for(i=0;i<=numPosEdges-1;i++) 00562 { 00563 TERMINAL_PRINTF("%9.3f",PosEdges[i]); 00564 if(i==numPosEdges-1) // when last data reached put in line return 00565 TERMINAL_PRINTF("\r\n"); 00566 else 00567 TERMINAL_PRINTF(", "); 00568 } 00569 00570 } 00571 00572 void reviewEdges() 00573 { 00574 LastTrackStatus = CurrentTrackStatus; 00575 00576 if ((numPosEdges == 1) && (numNegEdges == 1)) // only one negative and positive edge found (LINE) 00577 { 00578 if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) // has proper expected width 00579 { 00580 CurrentTrackStatus = LineFound; // report line found! 00581 UnknownCount = 0; // reset unknown status count 00582 LastLinePosition = CurrentLinePosition; 00583 CurrentLinePosition = (PosEdges[0]+NegEdges[0]) / 2; // update line position 00584 } 00585 } else if ((numPosEdges == 1) && (numNegEdges == 0)) { // 1 pos edge found (POSSIBLE LINE) 00586 if ((PosEdges[0] <= MAX_LINE_WIDTH) && (LastLinePosError < 0)) // pos edge is within line width of edge of camera (LEFT) 00587 { 00588 CurrentTrackStatus = LineFound; // report line found! 00589 UnknownCount = 0; // reset unknown status count 00590 LastLinePosition = CurrentLinePosition; 00591 CurrentLinePosition = PosEdges[0] - ( MAX_LINE_WIDTH / 2); // update line position 00592 // TERMINAL_PRINTF("*** SINGLE POSEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); 00593 } 00594 } else if ((numNegEdges == 1) && (numPosEdges == 0)) { // 1 neg edge found (POSSIBLE LINE) 00595 if ((NegEdges[0] >= (MAX_LINE_SCAN - MAX_LINE_WIDTH)) && (LastLinePosError > 0)) // neg edge is within line width of edge of camera (RIGHT) 00596 { 00597 CurrentTrackStatus = LineFound; // report line found! 00598 UnknownCount = 0; // reset unknown status count 00599 LastLinePosition = CurrentLinePosition; 00600 CurrentLinePosition = NegEdges[0] + ( MAX_LINE_WIDTH / 2); // update line position 00601 // TERMINAL_PRINTF("*** SINGLE NEGEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); 00602 } 00603 } else if ((numPosEdges == 2) && (numNegEdges == 2)) { // 2 negative and 2 positive edges found (STARTING/FINISH GATE) 00604 00605 if ((((NegEdges[0] - PosEdges[0]) >= MIN_LINE_WIDTH) && ((NegEdges[0] - PosEdges[0]) <= MAX_LINE_WIDTH)) && // white left 'line' 00606 (((NegEdges[1] - PosEdges[1]) >= MIN_LINE_WIDTH) && ((NegEdges[1] - PosEdges[1]) <= MAX_LINE_WIDTH)) && // white right 'line' 00607 (((PosEdges[1] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH)) // actual track line 00608 ) 00609 00610 00611 if (startRaceTicker > STARTGATEDELAY) { // only start counting for starting gate until after delay 00612 StartGateFoundCount++; 00613 } 00614 00615 CurrentTrackStatus = StartGateFound; 00616 UnknownCount = 0; // reset unknown status count 00617 00618 } else if ((numPosEdges > 1) && (numNegEdges > 1)) { // more than 1 negative edge and positive edge found (but not 2 for both) (STARTING / FINISH GATE) 00619 00620 // remove edges that aren't close to center TBD DDHH 00621 00622 if (terminalOutput) { 00623 TERMINAL_PRINTF("***************************************** \r\n"); 00624 TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n"); 00625 TERMINAL_PRINTF("***************************************** \r\n"); 00626 } 00627 CurrentTrackStatus = Unknown; 00628 00629 } else { // no track or starting gate found 00630 00631 if (terminalOutput) { 00632 TERMINAL_PRINTF("***************************************** \r\n"); 00633 TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n", CurrentLinePosition); 00634 TERMINAL_PRINTF("***************************************** \r\n"); 00635 } 00636 00637 CurrentTrackStatus = Unknown; 00638 UnknownCount++; 00639 } 00640 00641 00642 00643 00644 } 00645 00646 void ActOnTrackStatus() 00647 { 00648 // Decide what to do next based on current track status 00649 00650 if (CurrentTrackStatus == LineFound) { // LINE FOUND! 00651 00652 if (terminalOutput) { 00653 TERMINAL_PRINTF("***************************************** \r\n"); 00654 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); 00655 TERMINAL_PRINTF("***************************************** \r\n"); 00656 } 00657 00658 // Update steering position 00659 SteeringControl(); 00660 00661 // Apply to servo 00662 Steer(); 00663 00664 } else if (CurrentTrackStatus == StartGateFound) { // STARTING GATE FOUND 00665 00666 if (terminalOutput) { 00667 TERMINAL_PRINTF("***************************************** \r\n"); 00668 TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n"); 00669 TERMINAL_PRINTF("********** count = %d ********** \r\n", StartGateFoundCount); 00670 TERMINAL_PRINTF("***************************************** \r\n"); 00671 } 00672 00673 // END RACE! 00674 if (startGateStop) { 00675 if (StartGateFoundCount > STARTGATEFOUNDMAX) 00676 { 00677 go = false; // STOP!! 00678 } 00679 } 00680 00681 } 00682 00683 00684 00685 } 00686 00687 void SteeringControl() 00688 { 00689 float ReadPot1 = 1; 00690 float targetPosition = (float)( (NUM_LINE_SCAN / 2) - 0.5); // target to achieve for line position 00691 00692 float KP; // proportional control factor 00693 float KI; // integral control factor 00694 float KD; // derivative control factor 00695 00696 float Pout, Iout, Dout; // PID terms 00697 00698 // Calculate error 00699 // make error to the right positive 00700 // i.e. if LINE to the right-- then CurrentLinePosError > 0 00701 // if LINE to the left -- then CurrentLinePosError < 0 00702 CurrentLinePosError = CurrentLinePosition - targetPosition; 00703 00704 // Get absolute error 00705 if (CurrentLinePosError >= 0) 00706 AbsError = CurrentLinePosError; 00707 else 00708 AbsError = -1 * CurrentLinePosError; 00709 00710 // CHOOSE SET OF PID CONTROL PARAMETERS 00711 switch (CONTROL_METHOD) { 00712 case 0: 00713 // Pure proportional control based on range of steering values vs. range of error values 00714 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH ); 00715 KD = 0; 00716 KI = 0; 00717 break; 00718 case 1: 00719 // Proportional control with 50% bit more oomph --- a bit more aggressive around the bends 00720 ReadPot1 = TFC_ReadPot(1)+1; // pot range 0-2 00721 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH ); 00722 KP = KP * ReadPot1; 00723 KD = 0; 00724 KI = 0; 00725 break; 00726 case 2: // MANUAL TUNING CASE 1 (use pot to help determine tuning parameters) 00727 KP = TUNE_KP; 00728 KI = TUNE_KI; 00729 KD = TUNE_KD; 00730 case 3: 00731 if (AbsError < ABS_ERROR_THRESH) { 00732 KP = 0.003; // when relatively straight, keep KP gain low 00733 } else { 00734 KP = 0.010; // when curve begins or off track, increase KP gain 00735 } 00736 KI = 0; 00737 KD = 0; 00738 00739 default: 00740 break; 00741 } 00742 00743 /* Pseudocode 00744 previous_error = 0 00745 integral = 0 00746 start: 00747 error = setpoint - measured_value 00748 integral = integral + error*dt 00749 derivative = (error - previous_error)/dt 00750 output = Kp*error + Ki*integral + Kd*derivative 00751 previous_error = error 00752 wait(dt) 00753 goto start 00754 */ 00755 00756 00757 if (terminalOutput) { 00758 TERMINAL_PRINTF("KP = %6.4f\r\n", KP); 00759 TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition); 00760 } 00761 00762 00763 00764 // Update integral of error 00765 // i.e. if LINE stays to the right, then SumLinePosError increases 00766 // i.e. if LINE stays to the left, then SumLinePosError decreases 00767 SumLinePosError = SumLinePosError + ( CurrentLinePosError * DT ); 00768 00769 DerivError = (CurrentLinePosError - LastLinePosError) / DT; 00770 00771 if (terminalOutput) { 00772 TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n", CurrentLinePosition); 00773 TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n", CurrentLinePosError); 00774 } 00775 00776 // SECOND- calculate new servo position 00777 00778 // proportional control term 00779 Pout = KP * CurrentLinePosError; 00780 00781 // integral control term 00782 Iout = KI * SumLinePosError; 00783 00784 // Derivative control term 00785 Dout = KD * DerivError; 00786 00787 if (terminalOutput) { 00788 TERMINAL_PRINTF("KP = %6.4f\r\n", KP); 00789 TERMINAL_PRINTF("KI = %6.4f\r\n", KI); 00790 TERMINAL_PRINTF("KD = %6.4f\r\n", KD); 00791 TERMINAL_PRINTF("Pout = %6.4f\r\n", Pout); 00792 TERMINAL_PRINTF("Iout = %6.4f\r\n", Iout); 00793 TERMINAL_PRINTF("Dout = %6.4f\r\n", Dout); 00794 } 00795 00796 // Finally add offset to steering to account for non-centered servo mounting 00797 // CurrentSteerSetting = Pout + Iout + Dout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ); 00798 CurrentSteerSetting = Pout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ); 00799 00800 // store for next cycle deriv calculation 00801 LastLinePosError = CurrentLinePosError; 00802 00803 // for tuning control algo only 00804 if (1 == 0) { 00805 TERMINAL_PRINTF("*** ******************************** \r\n"); 00806 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); 00807 TERMINAL_PRINTF("*** ERROR %9.3f *** \r\n", CurrentLinePosError); 00808 TERMINAL_PRINTF("*** INTEGRAL ERROR %9.3f *** \r\n", SumLinePosError); 00809 TERMINAL_PRINTF("*** DERIVATIVE ERROR %9.3f *** \r\n", DerivError); 00810 TERMINAL_PRINTF("*** P STEER SETTING %9.3f *** \r\n", CurrentSteerSetting); 00811 TERMINAL_PRINTF("*** PI STEER SETTING %9.3f *** \r\n", (CurrentSteerSetting + Iout)); 00812 TERMINAL_PRINTF("*** ******************************** \r\n"); 00813 wait_ms(1000); 00814 } 00815 00816 } 00817 00818 void Steer() 00819 { 00820 00821 // make sure doesn't go beyond steering limits 00822 if (CurrentSteerSetting > MAX_STEER_RIGHT) 00823 { 00824 CurrentSteerSetting = MAX_STEER_RIGHT; 00825 } else if (CurrentSteerSetting < MAX_STEER_LEFT) 00826 { 00827 CurrentSteerSetting = MAX_STEER_LEFT; 00828 } 00829 00830 if (terminalOutput) { 00831 TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n", CurrentSteerSetting); 00832 } 00833 TFC_SetServo(0,CurrentSteerSetting); 00834 00835 } 00836 00837 void SpeedControl() 00838 { 00839 00840 // Get max speed setting from reading pot0 00841 // then adjust 00842 00843 float ErrLimit; 00844 float LeftDriveRatio, RightDriveRatio; 00845 00846 // set maximum speed 00847 if (doRisky) 00848 MaxSpeed = LUDICROUS_SPEED * ((TFC_ReadPot(0)+1)/2.0); // faster 00849 else 00850 MaxSpeed = LIGHT_SPEED * ((TFC_ReadPot(0)+1)/2.0); // slower 00851 00852 switch (SPEED_ADJUST) 00853 { 00854 case 0: 00855 // SPEED ADJUST METHOD 0 00856 // no speed adjust 00857 LeftDriveRatio = MAX_POWER; 00858 RightDriveRatio = LeftDriveRatio; 00859 case 1: 00860 // SPEED ADJUST METHOD 1 00861 // High speed when error is low, low speed when error is high 00862 // lower speed when more than third outside of center 00863 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.33; 00864 if (AbsError > ErrLimit) { 00865 LeftDriveRatio = MIN_POWER; 00866 } else { 00867 LeftDriveRatio = MAX_POWER; 00868 } 00869 RightDriveRatio = LeftDriveRatio; 00870 break; 00871 case 2: 00872 // SPEED ADJUST METHOD 2 00873 // Have max/min speed adjust proportional to absolute value of line error 00874 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO; 00875 LeftDriveRatio = MAX_POWER - ((MAX_POWER - MIN_POWER) * (AbsError / ErrLimit)); 00876 RightDriveRatio = LeftDriveRatio; 00877 break; 00878 case 3: 00879 // SPEED ADJUST METHOD 3 00880 // have wheel relative speed proportional to absolute value of line error 00881 if (CurrentLinePosError > 0) { // heading right, slow right wheel down 00882 LeftDriveRatio = MAX_POWER; 00883 RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); 00884 } else if (CurrentLinePosError < 0) { // heading left, slow left wheel down 00885 LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); // note sign change due to error being negative 00886 RightDriveRatio = MAX_POWER; 00887 } else { 00888 LeftDriveRatio = MAX_POWER; 00889 RightDriveRatio = MAX_POWER; 00890 } 00891 break; 00892 case 4: 00893 // SPEED ADJUST METHOD 4 00894 // have wheel relative speed proportional to absolute value of line error 00895 // only when above a certain error 00896 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.1; 00897 if (CurrentLinePosError > ErrLimit) { // right turn-- slow right wheel down a bit 00898 LeftDriveRatio = MAX_POWER; 00899 RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); 00900 } else if (CurrentLinePosError < (-1 * ErrLimit)) { // left turn-- slow left wheel down a bit 00901 LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); // note sign change due to error being negative 00902 RightDriveRatio = MAX_POWER; 00903 } else { // when in center drive full speed 00904 LeftDriveRatio = MAX_POWER; 00905 RightDriveRatio = MAX_POWER; 00906 } 00907 break; 00908 case 5: 00909 // SPEED ADJUST METHOD 5 00910 // High speed when error is low, low speed when error is high 00911 // lower speed when more than third outside of center 00912 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.2; 00913 if (AbsError > ErrLimit) { 00914 LeftDriveRatio = MIN_POWER; 00915 } else { 00916 LeftDriveRatio = MAX_POWER; 00917 } 00918 RightDriveRatio = LeftDriveRatio; 00919 break; 00920 case 6: 00921 // SPEED ADJUST METHOD 6 00922 // High speed when error is low, low speed when error is high 00923 // lower speed when more than third outside of center 00924 if (AbsError > ABS_ERROR_THRESH) { 00925 LeftDriveRatio = MIN_POWER; 00926 } else { 00927 LeftDriveRatio = MAX_POWER; 00928 } 00929 RightDriveRatio = LeftDriveRatio; 00930 break; 00931 default: 00932 break; 00933 00934 } 00935 // TBD-- add speed adjust based on Xaccel sensor! 00936 00937 00938 // currently no control mechanism as don't have speed sensor 00939 CurrentLeftDriveSetting = (float) (LeftDriveRatio / 100) * MaxSpeed; 00940 CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed; 00941 00942 00943 if (terminalOutput) { 00944 TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError); 00945 TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit); 00946 TERMINAL_PRINTF("MAX SPEED = %5.2f\r\n", MaxSpeed); 00947 TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting); 00948 TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting); 00949 } 00950 00951 } 00952 00953 void Drive() 00954 { 00955 00956 // START! 00957 // if not going, go when button A is pressed 00958 if (!go) { 00959 if(TFC_PUSH_BUTTON_0_PRESSED) { 00960 go = true; 00961 UnknownCount = 0; 00962 StartGateFoundCount = 0; 00963 startRaceTicker = TFC_Ticker[0]; // keep track of start of race 00964 logDataIndex = 0; // reset log data index 00965 } 00966 } 00967 00968 // STOP! 00969 // if going, stop when button A is pressed 00970 if (go) { 00971 if(TFC_PUSH_BUTTON_1_PRESSED) { 00972 go = false; 00973 StartGateFoundCount = 0; 00974 } 00975 } 00976 00977 // EMERGENCY STOP! 00978 // 'kill switch' to prevent crashes off-track 00979 if (killSwitch) { 00980 if (UnknownCount > UNKNOWN_COUNT_MAX) { // if track not found after certain time 00981 go = false; // kill engine 00982 StartGateFoundCount = 0; 00983 } 00984 } 00985 00986 // **************** 00987 00988 if (!go) { // stop! 00989 TFC_SetMotorPWM(0,0); //Make sure motors are off 00990 TFC_HBRIDGE_DISABLE; 00991 } 00992 00993 if (go) { // go! 00994 TFC_HBRIDGE_ENABLE; 00995 // motor A = right, motor B = left based on way it is mounted 00996 TFC_SetMotorPWM(CurrentRightDriveSetting,CurrentLeftDriveSetting); 00997 } 00998 } 00999 01000 01001 void adjustLights() 01002 { 01003 01004 // LIGHT ADJUST METHOD 1 01005 // threshold is 1/5 of light intensity 'range' 01006 if (1 == 0) { 01007 DerivThreshold = (float) (MaxLightIntensity - MinLightIntensity) / 5; 01008 NegDerivThreshold = (float) -1 * (DerivThreshold); 01009 PosDerivThreshold = (float) (DerivThreshold); 01010 } else { 01011 // LIGHT ADJUST METHOD 2 -- SEEMS TO WORK MUCH BETTER 01012 // pos edge threshold is half range of max deriv above aver derive 01013 // neg edge threshold is half range of min deriv above aver derive 01014 01015 NegDerivThreshold = (float) (minDerVal - aveDerVal) * DER_RATIO; 01016 PosDerivThreshold = (float) (maxDerVal - aveDerVal) * DER_RATIO; 01017 01018 } 01019 01020 printAdjustLightsData(); 01021 01022 } 01023 01024 void printAdjustLightsData() 01025 { 01026 if (terminalOutput) { 01027 TERMINAL_PRINTF("Max Light Intensity: %4d\r\n", MaxLightIntensity); 01028 TERMINAL_PRINTF("Min Light Intensity: %4d\r\n", MinLightIntensity); 01029 TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", DerivThreshold); 01030 } 01031 01032 } 01033 01034 void feedbackLights() 01035 { 01036 switch (CurrentTrackStatus) 01037 { 01038 case LineFound: 01039 TFC_BAT_LED0_OFF; 01040 TFC_BAT_LED1_ON; 01041 TFC_BAT_LED2_ON; 01042 TFC_BAT_LED3_OFF; 01043 break; 01044 case StartGateFound: 01045 TFC_BAT_LED0_ON; 01046 TFC_BAT_LED1_OFF; 01047 TFC_BAT_LED2_OFF; 01048 TFC_BAT_LED3_ON; 01049 break; 01050 default: 01051 TFC_BAT_LED0_OFF; 01052 TFC_BAT_LED1_OFF; 01053 TFC_BAT_LED2_OFF; 01054 TFC_BAT_LED3_OFF; 01055 } 01056 01057 } 01058
Generated on Sat Jul 16 2022 21:06:43 by
1.7.2