This is an example program that actually allows the car to race using the FRDM-TFC library!

Dependencies:   FRDM-TFC

Fork of TFC-RACING-DEMO by Daniel Hadad

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Spices.cpp Source File

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