Shumelev dima
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.51 // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach) 00019 #define MAX_STEER_RIGHT 0.39 // 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_SPEED 0.7 00027 #define TUNE_KP 0.008 00028 #define TUNE_KI 0 00029 #define TUNE_KD 0 00030 #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 00031 #define SPEED_ADJUST 4 00032 #define ABS_ERROR_THRESH 10 // number of pixels line position offset before changing KP value 00033 #define CONTROL_METHOD 2 // which control method to use 00034 00035 00036 // Drive/motor params 00037 // 0.4 way too slow!! need to charge battery 00038 #define SUB_LIGHT_SPEED 0.5 // moderate speed (value 0 to 1 sent to motors) 00039 #define LIGHT_SPEED 0.6 // fast... 00040 #define RIDICULOUS_SPEED 0.7 // faster... 00041 #define LUDICROUS_SPEED 0.9 // faster still! 00042 #define MAX_POWER 100 // percent max power (for speed adjustments) 00043 00044 // algo params 00045 #define UNKNOWN_COUNT_MAX 50 // max value to allow for unknown track conditions before killing engine 00046 #define STARTGATEFOUNDMAX 0 // max value to allow for finding starting gate before killing engine 00047 #define STARTGATEDELAY 50 // Delay before searching for starting gate to kill engine 00048 00049 #define MMA8451_I2C_ADDRESS (0x1d<<1) // address for accelerometer? 00050 00051 00052 /* CAR INTERFACE 00053 00054 DIP SWITCH: 00055 ----------------------------------------------------------------- 00056 1 - ON: Run MCP below; OFF: Run Demo program (see main.cpp) 00057 2 - ON: Log frame data to array 00058 3 - ON: Risky race option; OFF: Conservative race option 00059 4 - ON: Start Gate Kill Switch Active 00060 00061 POTS 00062 ----------------------------------------------------------------- 00063 0 - controls nothing at the moment 00064 1 - controls nothing at the moment 00065 00066 PUSHBUTTONS 00067 ----------------------------------------------------------------- 00068 A - START car race! 00069 B - END CAR RACE / (while holding down when 'log frame data' active will also output terminal data) 00070 00071 */ 00072 00073 // LEARNING CAR CLUB 9/10/13: 00074 // IP need to test-- get around U turns -- more aggressive proportional control? Or derivative? Have camera look farther ahead but not too far ahead 00075 // IP need to test -- fix lighting for tunnels (if good algo doesn't need lighting!!) 00076 // -- increase power up to get up hills, brake down hills (accel: http://mbed.org/users/SomeRandomBloke/code/MMA8451Q/#) 00077 // -- add speed control? (hall effect sensor) 00078 // DONE -- make sure steering won't go past limits!! 00079 // 9/12/13 - adjust camera exposure time based on maximum light intensity!! 00080 // 9/14/13 - DONE -- make derivative threshold related to maximum light intensity! 00081 // 9/16/13 - crash at car club blew out resistor R8, replaced it and getty twitching when powering servo from USB only. Ok when powering from battery. 00082 // 9/17/13 - experiments show that derivative control just doesn't work very well-- the tinest error delta causes huge drastic changes, need to use non-linear proportional 00083 // control instead... parabolic?? 00084 // DONE -- Also need to slow down on curves 00085 // speed up hills and slow down on downhill... 00086 // measure speed? 00087 // 9/18/13 - Test track work: doesn't appear to be sampling camera fast enough-- not able to handle the wiggly track and sometimes not able to handle the curves! 00088 // TODO: Need to increase rate at which camera sampled and decisions are made!! Look to codewarrior code 00089 // Need to cut off left/right ends of camera data-- seems to not read line properly in well lit rooms 00090 // DONE Speed adjust as you go round; Need to have it slow down "into" curve, speed up again "out of curve" 00091 // 9/23/13 - Definitely not processing camera fast enough-- seems to not react well when speed up the car. Goes slow just fine all the way 'round. 00092 // TODO: Need to see how often TFC_LineScanImageReady gets updated. If update camera sample freq how will that impact exposure? Light adjustment algos should 00093 // be able to handle it. Need to be able to measure the 'processing time' required. Also wondering if 20mS is fast enough for updating the servo?? Need 00094 // to calculate how fast servo needs to really go based on track curves and speed. 00095 // TODO: Need a way to measure speed I think as well. 00096 // TODO: Need to control speed differentially across the different motors!! See Eli video!! 00097 // TODO: Use this to get excel feedback quicker: http://strokescribe.com/en/serial-port-download.html (Doesn't work too well-- prefer my own excel method) 00098 // TODO: Use PID control for steering! 00099 // TODO: Use Speed control (PID?) --- add speed sensor! 00100 // 10/8/13 Reduced speed control to 90% (was 85%)-- seems to go off track and lose line at high speed-- mainly on the U turns 00101 // Need to figure out why derivative control in steering doesn't work well. Add integral control. 00102 // Latest track times (no hill, no bumps, no tunnel, only squiggles) = 11.8sec at high speed with speed control 00103 // Losing time on the curves-- need to optimize!! 00104 // Worry about hill later-- need to get track times down around 8sec first. 00105 // ***************************** 00106 // ** Implement some method to acount for U-TURNS on track-- and to help even go beyond what camera can see as far as estimating line position 00107 // ** (U-TURNS take the longest time out of track) 00108 // ** Method to help find the line even when not really visible 00109 // ***************************** 00110 // 10/17/13 00111 // NEED CURVE AND WIGGLE DETECT!! 00112 // 10/20/13 Added logging capability and 'algo time' detect 00113 // It is not finding the line on the edges when on a curve-- likely because my line detect algo requires both edges 00114 // TODO-- need method that can use only one edge! 00115 00116 // TODO LIST 00117 // - Speed Control via Sensor WAITING ON SENSOR 00118 // - Differential drive around curves DONE -- still trying to figure out what %age to drop on turns 00119 // - Full PID steering control IN PROGRESS 00120 // - Starting gate kill engine debug IN PROGRESS -- need to add delay 00121 // - Off track kill engine debug -- IN PROGRESS-- seems to kill car prematurely 00122 00123 // image processing vars 00124 uint16_t GrabLineScanImage0[NUM_LINE_SCAN]; // snapshot of camera data for this 'frame' 00125 float DerivLineScanImage0[NUM_LINE_SCAN]; // derivative of line scan data 00126 float NegEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data negative edges found 00127 float PosEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data positive edges found 00128 uint16_t numNegEdges = 0, numPosEdges = 0; // max value of valid neg and positive indices (also serves as a count of # edges found) 00129 uint16_t MaxLightIntensity = 0; // max measured light intensity -- to account for lighting differences 00130 uint16_t MinLightIntensity = (1 << 12); // min measured light intensity -- to account for lighting differences 00131 float maxDerVal = 0; // max deriv value 00132 float minDerVal = (float) (1 << 12); // min deriv value 00133 float aveDerVal = 0; // average deriv value 00134 float DerivThreshold = (1 << 9); // Derivative Threshold (default) 00135 float PosDerivThreshold = (1 << 9); // Pos Edge Derivative Threshold (default) 00136 float NegDerivThreshold = (1 << 9); // Neg Edge Derivative Threshold (default) 00137 00138 00139 // Steering control variables 00140 float CurrentLinePosition; // Current position of track line (in pixels -- 0 to 127) 00141 float LastLinePosition; // Last position of track line (in pixels -- 0 to 127) 00142 float CurrentLinePosError = 0; // Current line position error (used for derivative calc) 00143 float AbsError; 00144 float LastLinePosError = 0; // Last line position error (used for derivative calc) 00145 float SumLinePosError = 0; // Sum of line position error (used for integral calc) 00146 float DerivError = 0; // Derivative of error 00147 float CurrentSteerSetting = (MAX_STEER_RIGHT + MAX_STEER_LEFT) / 2; // drive straight at first 00148 float CurrentLeftDriveSetting = 0; // Drive setting (left wheel) 00149 float CurrentRightDriveSetting = 0; // Drive setting (right wheel) 00150 00151 // Speed control vars 00152 float MaxSpeed; // maximum speed allowed 00153 00154 uint16_t startRaceTicker; // ticker at start of race1 00155 00156 // Custom Data Types 00157 typedef enum TrackStatusType {Unknown, 00158 LineFound, 00159 StartGateFound, 00160 LineJustLeft} TrackStatusType; 00161 00162 TrackStatusType CurrentTrackStatus; // current track status 00163 TrackStatusType LastTrackStatus; // last track status 00164 00165 /* typedef enum TrackType {NotSure, 00166 Straight, 00167 Curve, 00168 Wiggle, 00169 Bumps, 00170 StartGate, 00171 UpHill, 00172 DownHill} TrackType; 00173 00174 TrackType CurrentTrack; */ 00175 00176 00177 struct LogData { 00178 float linepos; 00179 float steersetting; 00180 float leftdrivesetting; 00181 float rightdrivesetting; 00182 }; 00183 00184 LogData frameLogs[NUM_LOG_FRAMES]; // array of log data to store 00185 int logDataIndex; // index for log data 00186 00187 00188 int StartGateFoundCount = 0; // how many times start gate has been found 00189 int UnknownCount = 0; // how many times nothing has been found (to help with kill switch implementation) 00190 bool go = false; // Car can go! Should be set to false to start. 00191 00192 // EXTRA CONTROL PARAMETERS 00193 bool debugFakeMode = false; // if true, ignores real camera and uses fake camera input instead; used for data processing debug 00194 int terminalOutput = 0; // set debug level for terminal output 00195 // 0 : no terminal output, race! 00196 // 1 : output just to measure frame rate 00197 // 2 : output for measuring time of operations 00198 // 3 : output with delay 00199 bool doLogData = false; // whether to capture log data to output later on 00200 bool killSwitch = false; // whether to enable Kill Switch (allow engine to stop after not finding track) 00201 bool startGateStop = false; // whether to stop or not depending on starting gate reading 00202 bool doRisky = false; // race style-- whether conservative or risky 00203 00204 // timer stuff 00205 Timer timer; 00206 int after_time, before_time, start_time, last_start_time; 00207 bool run_once = false; 00208 00209 void MasterControlProgram() 00210 { 00211 00212 // put here all things want to run only once after reset 00213 if (!run_once){ 00214 if ((terminalOutput == 1) || (terminalOutput == 2)){ 00215 timer.start(); 00216 } 00217 run_once = true; 00218 } 00219 00220 // read DIP switches and Pots for data 00221 readSwitches(); 00222 00223 // Every 4s (or Terminal Output is off, i.e. race mode!) 00224 // AND line scan image ready (or fake mode where image is always ready) 00225 // (ticker updates every 2ms) (Line scan image ready very 20mS?) 00226 if((TFC_Ticker[0]>2000 || (terminalOutput != 3)) && (TFC_LineScanImageReady>0 || debugFakeMode)) 00227 { 00228 00229 // stuff that needs to be reset with each image frame 00230 if (terminalOutput == 1) { 00231 last_start_time = start_time; 00232 start_time = timer.read_us(); 00233 TERMINAL_PRINTF("TIME:Between frames:%d:uSec\r\n", start_time - last_start_time); 00234 before_time = timer.read_us(); 00235 } 00236 TFC_Ticker[0] = 0; 00237 TFC_LineScanImageReady=0; // must reset to 0 after detecting non-zero 00238 MaxLightIntensity = 0; // reset 00239 MinLightIntensity = (1 << 12); // reset 00240 00241 // grab camera frame 00242 grabCameraFrame(); 00243 00244 if (terminalOutput == 2) { 00245 after_time = timer.read_us(); 00246 TERMINAL_PRINTF("TIME:TO AFTER grabCameraFrame:%d:uSec\r\n", after_time - before_time); 00247 before_time = timer.read_us(); 00248 } 00249 00250 // calcalate derivative of linescandata, filter starttime data 00251 derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]); 00252 00253 if (terminalOutput == 2) { 00254 after_time = timer.read_us(); 00255 TERMINAL_PRINTF("TIME:TO AFTER derivativeLineScan:%d:uSec\r\n", after_time - before_time); 00256 before_time = timer.read_us(); 00257 } 00258 00259 // adjust deriv threshold based on max lighting value 00260 // has to be called before find edges 00261 adjustLights(); 00262 00263 if (terminalOutput == 2) { 00264 after_time = timer.read_us(); 00265 TERMINAL_PRINTF("TIME:TO AFTER adjustLights:%d:uSec\r\n", after_time - before_time); 00266 before_time = timer.read_us(); 00267 } 00268 00269 //find edges from derivative data 00270 findEdges_v2(&DerivLineScanImage0[0]); 00271 00272 if (terminalOutput == 2) { 00273 after_time = timer.read_us(); 00274 TERMINAL_PRINTF("TIME:TO AFTER findEdges_v2:%d:uSec\r\n", after_time - before_time); 00275 before_time = timer.read_us(); 00276 } 00277 00278 // turn on terminal output if line not found -- FOR DEBUG 00279 //if (CurrentTrackStatus == Unknown) 00280 // terminalOutput = 1; 00281 00282 //review edge data and set position or starting flag appropriately 00283 reviewEdges(); 00284 00285 if (terminalOutput == 2) { 00286 after_time = timer.read_us(); 00287 TERMINAL_PRINTF("TIME:TO AFTER reviewEdges:%d:uSec\r\n", after_time - before_time); 00288 before_time = timer.read_us(); 00289 } 00290 00291 if (terminalOutput == 3) { 00292 // print data to Terminal for camera 0 00293 printLineScanData(&GrabLineScanImage0[0]); 00294 00295 // print deriviative of linescandata, filter starttime data 00296 printDerivLineScanData(&DerivLineScanImage0[0]); 00297 00298 printAdjustLightsData(); 00299 00300 printEdgesFound(); 00301 00302 } 00303 00304 // ** Track Status available at this point ** 00305 00306 00307 // test out accelerometer 00308 // accelTest(); 00309 00310 // Update things based on latest track status 00311 // e.g. change steering setting, stop car, ... 00312 ActOnTrackStatus(); 00313 00314 if (terminalOutput == 2) { 00315 after_time = timer.read_us(); 00316 TERMINAL_PRINTF("TIME:TO AFTER ActOnTrackStatus:%d:uSec\r\n", after_time - before_time); 00317 before_time = timer.read_us(); 00318 } 00319 00320 00321 //give LED feedback as to track status 00322 feedbackLights(); 00323 00324 if (terminalOutput == 2) { 00325 after_time = timer.read_us(); 00326 TERMINAL_PRINTF("TIME:TO AFTER feedbackLights:%d:uSec\r\n", after_time - before_time); 00327 before_time = timer.read_us(); 00328 } 00329 00330 // control max power (speed) 00331 SpeedControl(); 00332 00333 if (terminalOutput == 2) { 00334 after_time = timer.read_us(); 00335 TERMINAL_PRINTF("TIME:TO AFTER SpeedControl:%d:uSec\r\n", after_time - before_time); 00336 before_time = timer.read_us(); 00337 } 00338 00339 // Drive!! 00340 Drive(); 00341 00342 if (terminalOutput == 2) { 00343 after_time = timer.read_us(); 00344 TERMINAL_PRINTF("TIME:TO AFTER Drive:%d:uSec\r\n", after_time - before_time); 00345 before_time = timer.read_us(); 00346 } 00347 00348 // wait_ms(1); 00349 00350 // Capture Log data while driving 00351 if (go && doLogData) { 00352 captureData(); 00353 } 00354 00355 // Dump Log data to Terminal while stopped and holding B button 00356 if (!go && doLogData && TFC_PUSH_BUTTON_1_PRESSED) { 00357 dumpData(); 00358 } 00359 00360 if (terminalOutput == 2) { 00361 after_time = timer.read_us(); 00362 TERMINAL_PRINTF("TIME: ENTIRE FRAME (include prints):%d:uSec\r\n", after_time - start_time); 00363 before_time = timer.read_us(); 00364 } 00365 00366 if (terminalOutput == 3) { 00367 TERMINAL_PRINTF("\r\n**************************END********************************\r\n"); 00368 } 00369 00370 } 00371 } 00372 00373 void dumpData() 00374 { 00375 TERMINAL_PRINTF("INDEX,LINEPOS,STEERSETTING,LEFTDRIVESETTING,RIGHTDRIVESETTING\r\n"); 00376 for(logDataIndex=0;logDataIndex<NUM_LOG_FRAMES;logDataIndex++) { 00377 TERMINAL_PRINTF("%d,%6.2f,%6.2f,%6.2f,%6.2f\r\n",logDataIndex,frameLogs[logDataIndex].linepos,frameLogs[logDataIndex].steersetting,frameLogs[logDataIndex].leftdrivesetting,frameLogs[logDataIndex].rightdrivesetting); 00378 } 00379 } 00380 00381 void captureData() 00382 { 00383 frameLogs[logDataIndex].linepos = CurrentLinePosition; 00384 frameLogs[logDataIndex].steersetting = CurrentSteerSetting; 00385 frameLogs[logDataIndex].leftdrivesetting = CurrentLeftDriveSetting; 00386 frameLogs[logDataIndex].rightdrivesetting = CurrentRightDriveSetting; 00387 00388 // increment index 00389 logDataIndex++; 00390 if (logDataIndex > NUM_LOG_FRAMES) 00391 logDataIndex = 0; 00392 } 00393 00394 void readSwitches() 00395 { 00396 00397 // ********* GATHER DIP SWITCH INPUTS ********* 00398 if(TFC_GetDIP_Switch()&0x02) // SWITCH 2 00399 doLogData = true; // Log data to array 00400 else 00401 doLogData = false; // normal operation 00402 00403 if(TFC_GetDIP_Switch()&0x04) // SWITCH 3 00404 doRisky = true; 00405 else 00406 doRisky = false; 00407 00408 if(TFC_GetDIP_Switch()&0x08) // SWITCH 4 control start stop gate 00409 startGateStop = true; 00410 else 00411 startGateStop = false; 00412 00413 00414 } 00415 00416 void grabCameraFrame() 00417 { 00418 uint32_t i = 0; 00419 uint8_t fake_type = 4; // type of fake data if used 00420 00421 for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0 00422 { 00423 00424 if (debugFakeMode) { // use fake camera data 00425 switch (fake_type) { 00426 case 0: // ideal track -- line in center 00427 if (i<57 || i > 70) 00428 GrabLineScanImage0[i] = 0xFFF; // no line 00429 else 00430 GrabLineScanImage0[i] = 0x4B0; // line 00431 break; 00432 case 1: // ideal track -- line to the left 00433 if (i<27 || i > 40) 00434 GrabLineScanImage0[i] = 0xFFF; // no line 00435 else 00436 GrabLineScanImage0[i] = 0x4B0; // line 00437 break; 00438 case 2: // ideal track -- line to the right 00439 if (i<87 || i > 100) 00440 GrabLineScanImage0[i] = 0xFFF; // no line 00441 else 00442 GrabLineScanImage0[i] = 0x4B0; // line 00443 break; 00444 case 3: // ideal track -- starting gate! 00445 // TBD 00446 break; 00447 case 4: // less than ideal track -- debug multi-edge issue! 00448 if (i<54) 00449 GrabLineScanImage0[i] = 4000; // no line 00450 if (i == 54) 00451 GrabLineScanImage0[i] = 3370; // neg edge 00452 if (i == 55) 00453 GrabLineScanImage0[i] = 3309; // neg edge 00454 if (i == 56) 00455 GrabLineScanImage0[i] = 2016; // neg edge 00456 if (i == 57) 00457 GrabLineScanImage0[i] = 711; // neg edge 00458 if (i == 58) 00459 GrabLineScanImage0[i] = 696; // neg edge 00460 if ((i>58) && (i<69)) 00461 GrabLineScanImage0[i] = 500; // line 00462 if (i == 69) 00463 GrabLineScanImage0[i] = 1800; // pos edge 00464 if (i > 69) 00465 GrabLineScanImage0[i] = 4000; // no line 00466 default: 00467 break; 00468 } 00469 00470 } else { // use real camera data 00471 GrabLineScanImage0[i] = TFC_LineScanImage0[i]; 00472 } 00473 } 00474 00475 00476 } 00477 00478 void printLineScanData(uint16_t* LineScanData) 00479 { 00480 uint32_t i = 0; 00481 float Val; 00482 00483 TERMINAL_PRINTF("LINE SCAN DATA:,"); 00484 00485 for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0 00486 { 00487 if (1 == 1) { // use float to print 00488 Val = (float) LineScanData[i]; 00489 TERMINAL_PRINTF("%9.3f",Val); 00490 if(i==MAX_LINE_SCAN) // when last data reached put in line return 00491 TERMINAL_PRINTF("\r\n"); 00492 else 00493 TERMINAL_PRINTF(","); 00494 } else { 00495 TERMINAL_PRINTF("0x%X",LineScanData[i]); 00496 if(i==MAX_LINE_SCAN) // when last data reached put in line return 00497 TERMINAL_PRINTF("\r\n",LineScanData[i]); 00498 else 00499 TERMINAL_PRINTF(",",LineScanData[i]); 00500 } 00501 } 00502 00503 } 00504 00505 void printDerivLineScanData(float* derivLineScanData) 00506 { 00507 uint32_t i, minCnt = 0, maxCnt = 0; 00508 00509 minCnt = FILTER_ENDS; 00510 maxCnt = NUM_LINE_SCAN - FILTER_ENDS; 00511 00512 TERMINAL_PRINTF("DERIVATIVE DATA:,"); 00513 00514 for(i=minCnt;i<maxCnt;i++) // print one line worth of data (128) from Camera 0 00515 { 00516 TERMINAL_PRINTF("%9.3f",derivLineScanData[i]); 00517 if(i==maxCnt-1) // when last data reached put in line return 00518 TERMINAL_PRINTF("\r\n",derivLineScanData[i]); 00519 else 00520 TERMINAL_PRINTF(", ",derivLineScanData[i]); 00521 } 00522 00523 } 00524 00525 void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut) 00526 { 00527 00528 uint32_t i, minCnt = 0, maxCnt = 0; 00529 float DerVal, upperDerVal, lowerDerVal = 0; 00530 00531 maxDerVal = 0; 00532 minDerVal = (float) (1 << 12); 00533 aveDerVal = 0; 00534 00535 minCnt = FILTER_ENDS; 00536 maxCnt = NUM_LINE_SCAN - FILTER_ENDS; 00537 00538 // TERMINAL_PRINTF("i, upperDerVal, lowerDerVal, DerVal\r\n"); 00539 00540 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 00541 { 00542 00543 // store max light intensity value 00544 if (LineScanDataIn[i] > MaxLightIntensity) 00545 MaxLightIntensity = LineScanDataIn[i]; 00546 00547 // store min light intensity value 00548 if (LineScanDataIn[i] < MinLightIntensity) 00549 MinLightIntensity = LineScanDataIn[i]; 00550 00551 00552 // Central Derivative 00553 if (i==minCnt) { // start point 00554 upperDerVal = (float)(LineScanDataIn[i+1]); 00555 lowerDerVal = (float)(LineScanDataIn[i]); // make same as start point 00556 } else if (i==maxCnt - 1){ // end point 00557 upperDerVal = (float)(LineScanDataIn[i]); // make same as end point 00558 lowerDerVal = (float)(LineScanDataIn[i-1]); 00559 } else { // any other point 00560 upperDerVal = (float)(LineScanDataIn[i+1]); 00561 lowerDerVal = (float)(LineScanDataIn[i-1]); 00562 } 00563 DerVal = (upperDerVal - lowerDerVal) / 2; 00564 // TERMINAL_PRINTF("%d,%9.3f,%9.3f,%9.3f\r\n", i, upperDerVal, lowerDerVal, DerVal); 00565 00566 if (DerVal > maxDerVal) { 00567 maxDerVal = DerVal; 00568 } 00569 if (DerVal < minDerVal) { 00570 minDerVal = DerVal; 00571 } 00572 aveDerVal = aveDerVal + DerVal; //get sum 00573 DerivLineScanDataOut[i] = DerVal; 00574 } 00575 aveDerVal = (float) aveDerVal / (maxCnt - minCnt); 00576 } 00577 00578 //Not reliable for finding edges! 00579 void findEdges(float* derivLineScanData) 00580 { 00581 // search for edges in deriviative data using a threshold 00582 // need to store in a hash if that's possible... 00583 // combine edges that are a pixel apart 00584 00585 int i, minCnt = 0, maxCnt = 0; 00586 int multiNegEdgeCnt = 1, multiNegEdgeSum = 0; 00587 int multiPosEdgeCnt = 1, multiPosEdgeSum = 0; 00588 00589 minCnt = FILTER_ENDS; 00590 maxCnt = NUM_LINE_SCAN - FILTER_ENDS; 00591 00592 numNegEdges = 0; 00593 numPosEdges = 0; 00594 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 00595 { 00596 if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND! 00597 { 00598 if (terminalOutput == 3) { 00599 TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); 00600 } 00601 00602 if ((numNegEdges > 0) && (NegEdges[numNegEdges - 1] + 1 == i )) // if no multi edges 00603 { // edge actually across multiple pixels 00604 multiNegEdgeCnt++; 00605 multiNegEdgeSum = multiNegEdgeSum + i; 00606 if (terminalOutput == 3) { 00607 TERMINAL_PRINTF("MULTIEDGE FOUND! MultiNegEdgeCnt: %d; MultiNegEdgeSum: %d\r\n", multiNegEdgeCnt, multiNegEdgeSum); 00608 } 00609 } else { // not a multi-pixel edge known at this time, store negative edge index value 00610 numNegEdges++; 00611 if (terminalOutput == 3) { 00612 TERMINAL_PRINTF("NEG EDGE STORED WITH INDEX %d. NUM NEG EDGES = %d\r\n", i, numNegEdges); 00613 } 00614 NegEdges[numNegEdges - 1] = (float) i; 00615 multiNegEdgeSum = i; 00616 } 00617 00618 00619 } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND! 00620 00621 if (terminalOutput == 3) { 00622 TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); 00623 } 00624 00625 if ((numPosEdges > 0) && (PosEdges[numPosEdges - 1] + 1 == i )) 00626 { // edge actually across multiple pixels 00627 multiPosEdgeCnt++; 00628 multiPosEdgeSum = multiPosEdgeSum + i; 00629 if (terminalOutput == 3) { 00630 TERMINAL_PRINTF("MULTIEDGE FOUND! MultiPosEdgeCnt: %d; MultiPosEdgeSum: %d\r\n", multiPosEdgeCnt, multiPosEdgeSum); 00631 } 00632 } else { // not a multi-pixel edge known at this time, store Posative edge index value 00633 if (terminalOutput == 3) { 00634 TERMINAL_PRINTF("POS EDGE STORED WITH INDEX %d. NUM POS EDGES = %d\r\n", i, numPosEdges); 00635 } 00636 numPosEdges++; 00637 PosEdges[numPosEdges - 1] = (float) i; 00638 multiPosEdgeSum = i; 00639 } 00640 00641 } else { // NO EDGE FOUND 00642 // combine multi-edges if there are any 00643 if (multiNegEdgeCnt > 1) 00644 { 00645 NegEdges[numNegEdges - 1] = (float) multiNegEdgeSum / multiNegEdgeCnt; 00646 multiNegEdgeCnt = 1; multiNegEdgeSum = 0; 00647 } 00648 if (multiPosEdgeCnt > 1) 00649 { 00650 PosEdges[numPosEdges - 1] = (float) multiPosEdgeSum / multiPosEdgeCnt; 00651 multiPosEdgeCnt = 1; multiPosEdgeSum = 0; 00652 } 00653 00654 } 00655 } 00656 00657 } 00658 00659 00660 void findEdges_v2(float* derivLineScanData) 00661 { 00662 // search for edges in deriviative data using a threshold 00663 // need to store in a hash if that's possible... 00664 // combine edges that are a pixel apart 00665 00666 int i; 00667 00668 int NegEdgeBufCnt = 0, NegEdgeBufSum = 0; // serves as buffer to store neg edges found next to each other 00669 int PosEdgeBufCnt = 0, PosEdgeBufSum = 0; // serves as buffer to store pos edges found next to each other 00670 00671 int minCnt = FILTER_ENDS; 00672 int maxCnt = NUM_LINE_SCAN - FILTER_ENDS; 00673 00674 00675 00676 numNegEdges = 0; // count of neg edges found thus far 00677 numPosEdges = 0; // count of pos edges found thus far 00678 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 00679 { 00680 00681 if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND! 00682 { 00683 00684 if (terminalOutput == 3) { 00685 TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); 00686 } 00687 00688 NegEdgeBufCnt++; // add value to neg edge buffer 00689 NegEdgeBufSum = NegEdgeBufSum + i; 00690 00691 } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND! 00692 00693 if (terminalOutput == 3) { 00694 TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); 00695 } 00696 00697 PosEdgeBufCnt++; // add value to pos edge buffer 00698 PosEdgeBufSum = PosEdgeBufSum + i; 00699 00700 } else { // NO EDGE FOUND 00701 00702 // POP EDGE BUFFERS IF NON-EMPTY AND STORE TO EDGE "STACK" (i.e. edges found) 00703 00704 if (NegEdgeBufCnt > 0) { 00705 // store edge value 00706 numNegEdges++; 00707 NegEdges[numNegEdges - 1] = (float) NegEdgeBufSum / NegEdgeBufCnt; 00708 00709 // clear edge buffer 00710 NegEdgeBufSum = 0; NegEdgeBufCnt = 0; 00711 } 00712 00713 if (PosEdgeBufCnt > 0) { 00714 // store edge value 00715 numPosEdges++; 00716 PosEdges[numPosEdges - 1] = (float) PosEdgeBufSum / PosEdgeBufCnt; 00717 00718 // clear edge buffer 00719 PosEdgeBufSum = 0; PosEdgeBufCnt = 0; 00720 } 00721 00722 } 00723 00724 } 00725 00726 } 00727 00728 void printEdgesFound() 00729 { 00730 int i; 00731 00732 // Check that neg edges captured ok 00733 TERMINAL_PRINTF("NEGATIVE EDGES FOUND:,"); 00734 for(i=0;i<=numNegEdges-1;i++) 00735 { 00736 TERMINAL_PRINTF("%9.3f",NegEdges[i]); 00737 if(i==numNegEdges-1) // when last data reached put in line return 00738 TERMINAL_PRINTF("\r\n"); 00739 else 00740 TERMINAL_PRINTF(", "); 00741 } 00742 00743 00744 // Check that pos edges captured ok 00745 TERMINAL_PRINTF("POSITIVE EDGES FOUND:,"); 00746 for(i=0;i<=numPosEdges-1;i++) 00747 { 00748 TERMINAL_PRINTF("%9.3f",PosEdges[i]); 00749 if(i==numPosEdges-1) // when last data reached put in line return 00750 TERMINAL_PRINTF("\r\n"); 00751 else 00752 TERMINAL_PRINTF(", "); 00753 } 00754 00755 } 00756 00757 void reviewEdges() 00758 { 00759 LastTrackStatus = CurrentTrackStatus; 00760 00761 if ((numPosEdges == 1) && (numNegEdges == 1)) // only one negative and positive edge found (LINE) 00762 { 00763 if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) // has proper expected width 00764 { 00765 CurrentTrackStatus = LineFound; // report line found! 00766 UnknownCount = 0; // reset unknown status count 00767 LastLinePosition = CurrentLinePosition; 00768 CurrentLinePosition = (PosEdges[0]+NegEdges[0]) / 2; // update line position 00769 } 00770 } else if ((numPosEdges == 1) && (numNegEdges == 0)) { // 1 pos edge found (POSSIBLE LINE) 00771 if ((PosEdges[0] <= MAX_LINE_WIDTH) && (LastLinePosError < 0)) // pos edge is within line width of edge of camera (LEFT) 00772 { 00773 CurrentTrackStatus = LineFound; // report line found! 00774 UnknownCount = 0; // reset unknown status count 00775 LastLinePosition = CurrentLinePosition; 00776 CurrentLinePosition = PosEdges[0] - ( MAX_LINE_WIDTH / 2); // update line position 00777 // TERMINAL_PRINTF("*** SINGLE POSEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); 00778 } 00779 } else if ((numNegEdges == 1) && (numPosEdges == 0)) { // 1 neg edge found (POSSIBLE LINE) 00780 if ((NegEdges[0] >= (MAX_LINE_SCAN - MAX_LINE_WIDTH)) && (LastLinePosError > 0)) // neg edge is within line width of edge of camera (RIGHT) 00781 { 00782 CurrentTrackStatus = LineFound; // report line found! 00783 UnknownCount = 0; // reset unknown status count 00784 LastLinePosition = CurrentLinePosition; 00785 CurrentLinePosition = NegEdges[0] + ( MAX_LINE_WIDTH / 2); // update line position 00786 // TERMINAL_PRINTF("*** SINGLE NEGEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); 00787 } 00788 } else if ((numPosEdges == 2) && (numNegEdges == 2)) { // 2 negative and 2 positive edges found (STARTING/FINISH GATE) 00789 00790 if ((((NegEdges[0] - PosEdges[0]) >= MIN_LINE_WIDTH) && ((NegEdges[0] - PosEdges[0]) <= MAX_LINE_WIDTH)) && // white left 'line' 00791 (((NegEdges[1] - PosEdges[1]) >= MIN_LINE_WIDTH) && ((NegEdges[1] - PosEdges[1]) <= MAX_LINE_WIDTH)) && // white right 'line' 00792 (((PosEdges[1] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH)) // actual track line 00793 ) 00794 00795 00796 if (startRaceTicker > STARTGATEDELAY) { // only start counting for starting gate until after delay 00797 StartGateFoundCount++; 00798 } 00799 00800 CurrentTrackStatus = StartGateFound; 00801 UnknownCount = 0; // reset unknown status count 00802 00803 } else if ((numPosEdges > 1) && (numNegEdges > 1)) { // more than 1 negative edge and positive edge found (but not 2 for both) (STARTING / FINISH GATE) 00804 00805 // remove edges that aren't close to center TBD DDHH 00806 00807 if (terminalOutput == 3) { 00808 TERMINAL_PRINTF("***************************************** \r\n"); 00809 TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n"); 00810 TERMINAL_PRINTF("***************************************** \r\n"); 00811 } 00812 CurrentTrackStatus = Unknown; 00813 00814 } else { // no track or starting gate found 00815 00816 if (terminalOutput == 3) { 00817 TERMINAL_PRINTF("***************************************** \r\n"); 00818 TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n", CurrentLinePosition); 00819 TERMINAL_PRINTF("***************************************** \r\n"); 00820 } 00821 00822 CurrentTrackStatus = Unknown; 00823 UnknownCount++; 00824 } 00825 00826 00827 00828 00829 } 00830 00831 void ActOnTrackStatus() 00832 { 00833 // Decide what to do next based on current track status 00834 00835 if (CurrentTrackStatus == LineFound) { // LINE FOUND! 00836 00837 if (terminalOutput == 3) { 00838 TERMINAL_PRINTF("***************************************** \r\n"); 00839 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); 00840 TERMINAL_PRINTF("***************************************** \r\n"); 00841 } 00842 00843 // Update steering position 00844 SteeringControl(); 00845 00846 // Apply to servo 00847 Steer(); 00848 00849 } else if (CurrentTrackStatus == StartGateFound) { // STARTING GATE FOUND 00850 00851 if (terminalOutput == 3) { 00852 TERMINAL_PRINTF("***************************************** \r\n"); 00853 TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n"); 00854 TERMINAL_PRINTF("********** count = %d ********** \r\n", StartGateFoundCount); 00855 TERMINAL_PRINTF("***************************************** \r\n"); 00856 } 00857 00858 // END RACE! 00859 if (startGateStop) { 00860 if (StartGateFoundCount > STARTGATEFOUNDMAX) 00861 { 00862 go = false; // STOP!! 00863 } 00864 } 00865 00866 } 00867 00868 00869 00870 } 00871 00872 void SteeringControl() 00873 { 00874 00875 float targetPosition = (float)( (NUM_LINE_SCAN / 2) - 0.5); // target to achieve for line position 00876 00877 float KP; // proportional control factor 00878 float KI; // integral control factor 00879 float KD; // derivative control factor 00880 00881 float Pout, Iout, Dout; // PID terms 00882 00883 // Calculate error 00884 // make error to the right positive 00885 // i.e. if LINE to the right-- then CurrentLinePosError > 0 00886 // if LINE to the left -- then CurrentLinePosError < 0 00887 CurrentLinePosError = CurrentLinePosition - targetPosition; 00888 00889 // Get absolute error 00890 if (CurrentLinePosError >= 0) 00891 AbsError = CurrentLinePosError; 00892 else 00893 AbsError = -1 * CurrentLinePosError; 00894 00895 // CHOOSE SET OF PID CONTROL PARAMETERS 00896 switch (CONTROL_METHOD) { 00897 case 0: 00898 // Pure proportional control based on range of steering values vs. range of error values 00899 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH ); 00900 KD = 0; 00901 KI = 0; 00902 break; 00903 case 1: 00904 // Proportional control with 50% bit more oomph --- a bit more aggressive around the bends 00905 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH ); 00906 KP = KP * 1.5; 00907 KD = 0; 00908 KI = 0; 00909 break; 00910 case 2: // MANUAL TUNING CASE 1 (use pot to help determine tuning parameters) 00911 KP = TUNE_KP; 00912 KI = TUNE_KI; 00913 KD = TUNE_KD; 00914 case 3: 00915 if (AbsError < ABS_ERROR_THRESH) { 00916 KP = 0.003; // when relatively straight, keep KP gain low 00917 } else { 00918 KP = 0.010; // when curve begins or off track, increase KP gain 00919 } 00920 KI = 0; 00921 KD = 0; 00922 00923 default: 00924 break; 00925 } 00926 00927 /* Pseudocode 00928 previous_error = 0 00929 integral = 0 00930 start: 00931 error = setpoint - measured_value 00932 integral = integral + error*dt 00933 derivative = (error - previous_error)/dt 00934 output = Kp*error + Ki*integral + Kd*derivative 00935 previous_error = error 00936 wait(dt) 00937 goto start 00938 */ 00939 00940 00941 if (terminalOutput == 3) { 00942 TERMINAL_PRINTF("KP = %6.4f\r\n", KP); 00943 TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition); 00944 } 00945 00946 00947 00948 // Update integral of error 00949 // i.e. if LINE stays to the right, then SumLinePosError increases 00950 // i.e. if LINE stays to the left, then SumLinePosError decreases 00951 SumLinePosError = SumLinePosError + ( CurrentLinePosError * DT ); 00952 00953 DerivError = (CurrentLinePosError - LastLinePosError) / DT; 00954 00955 if (terminalOutput == 3) { 00956 TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n", CurrentLinePosition); 00957 TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n", CurrentLinePosError); 00958 } 00959 00960 // SECOND- calculate new servo position 00961 00962 // proportional control term 00963 Pout = KP * CurrentLinePosError; 00964 00965 // integral control term 00966 Iout = KI * SumLinePosError; 00967 00968 // Derivative control term 00969 Dout = KD * DerivError; 00970 00971 if (terminalOutput == 3) { 00972 TERMINAL_PRINTF("KP = %6.4f\r\n", KP); 00973 TERMINAL_PRINTF("KI = %6.4f\r\n", KI); 00974 TERMINAL_PRINTF("KD = %6.4f\r\n", KD); 00975 TERMINAL_PRINTF("Pout = %6.4f\r\n", Pout); 00976 TERMINAL_PRINTF("Iout = %6.4f\r\n", Iout); 00977 TERMINAL_PRINTF("Dout = %6.4f\r\n", Dout); 00978 } 00979 00980 // Finally add offset to steering to account for non-centered servo mounting 00981 // CurrentSteerSetting = Pout + Iout + Dout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ); 00982 CurrentSteerSetting = Pout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ); 00983 00984 // store for next cycle deriv calculation 00985 LastLinePosError = CurrentLinePosError; 00986 00987 // for tuning control algo only 00988 if (1 == 0) { 00989 TERMINAL_PRINTF("*** ******************************** \r\n"); 00990 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); 00991 TERMINAL_PRINTF("*** ERROR %9.3f *** \r\n", CurrentLinePosError); 00992 TERMINAL_PRINTF("*** INTEGRAL ERROR %9.3f *** \r\n", SumLinePosError); 00993 TERMINAL_PRINTF("*** DERIVATIVE ERROR %9.3f *** \r\n", DerivError); 00994 TERMINAL_PRINTF("*** P STEER SETTING %9.3f *** \r\n", CurrentSteerSetting); 00995 TERMINAL_PRINTF("*** PI STEER SETTING %9.3f *** \r\n", (CurrentSteerSetting + Iout)); 00996 TERMINAL_PRINTF("*** ******************************** \r\n"); 00997 wait_ms(1000); 00998 } 00999 01000 } 01001 01002 void Steer() 01003 { 01004 01005 // make sure doesn't go beyond steering limits 01006 if (CurrentSteerSetting > MAX_STEER_RIGHT) 01007 { 01008 CurrentSteerSetting = MAX_STEER_RIGHT; 01009 } else if (CurrentSteerSetting < MAX_STEER_LEFT) 01010 { 01011 CurrentSteerSetting = MAX_STEER_LEFT; 01012 } 01013 01014 if (terminalOutput == 3) { 01015 TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n", CurrentSteerSetting); 01016 } 01017 TFC_SetServo(0,CurrentSteerSetting); 01018 01019 } 01020 01021 void SpeedControl() 01022 { 01023 01024 // Get max speed setting from reading pot0 01025 // then adjust 01026 01027 float ErrLimit; 01028 float LeftDriveRatio, RightDriveRatio; 01029 01030 // set maximum speed allowed 01031 switch (1) 01032 { 01033 case 0: 01034 // read value off pot0 01035 MaxSpeed = TFC_ReadPot(0); 01036 break; 01037 case 1: 01038 if (doRisky) 01039 MaxSpeed = TUNE_SPEED + 0.1; 01040 else 01041 MaxSpeed = TUNE_SPEED; 01042 break; 01043 case 2: 01044 MaxSpeed = SUB_LIGHT_SPEED; 01045 break; 01046 case 3: 01047 MaxSpeed = LIGHT_SPEED; 01048 break; 01049 case 4: 01050 MaxSpeed = RIDICULOUS_SPEED; 01051 break; 01052 case 5: 01053 MaxSpeed = LUDICROUS_SPEED; 01054 break; 01055 default: 01056 break; 01057 } 01058 01059 switch (SPEED_ADJUST) 01060 { 01061 case 0: 01062 // SPEED ADJUST METHOD 0 01063 // no speed adjust 01064 LeftDriveRatio = MAX_POWER; 01065 RightDriveRatio = LeftDriveRatio; 01066 case 1: 01067 // SPEED ADJUST METHOD 1 01068 // High speed when error is low, low speed when error is high 01069 // lower speed when more than third outside of center 01070 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.33; 01071 if (AbsError > ErrLimit) { 01072 LeftDriveRatio = MIN_POWER; 01073 } else { 01074 LeftDriveRatio = MAX_POWER; 01075 } 01076 RightDriveRatio = LeftDriveRatio; 01077 break; 01078 case 2: 01079 // SPEED ADJUST METHOD 2 01080 // Have max/min speed adjust proportional to absolute value of line error 01081 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO; 01082 LeftDriveRatio = MAX_POWER - ((MAX_POWER - MIN_POWER) * (AbsError / ErrLimit)); 01083 RightDriveRatio = LeftDriveRatio; 01084 break; 01085 case 3: 01086 // SPEED ADJUST METHOD 3 01087 // have wheel relative speed proportional to absolute value of line error 01088 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO; 01089 if (CurrentLinePosError > 0) { // heading right 01090 LeftDriveRatio = MAX_POWER; 01091 RightDriveRatio = (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER; 01092 } else if (CurrentLinePosError < 0) { // heading left 01093 RightDriveRatio = MAX_POWER; 01094 LeftDriveRatio = (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER; 01095 } else { 01096 LeftDriveRatio = MAX_POWER; 01097 RightDriveRatio = MAX_POWER; 01098 } 01099 break; 01100 case 4: 01101 // SPEED ADJUST METHOD 4 01102 // have wheel relative speed proportional to absolute value of line error 01103 // only when above a certain error 01104 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.1; 01105 if (CurrentLinePosError > ErrLimit) { // heading right 01106 LeftDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); 01107 RightDriveRatio = MIN_POWER; 01108 } else if (CurrentLinePosError < (-1 * ErrLimit)) { // heading left 01109 RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); 01110 LeftDriveRatio = MIN_POWER; 01111 } else { 01112 LeftDriveRatio = MAX_POWER; 01113 RightDriveRatio = MAX_POWER; 01114 } 01115 break; 01116 case 5: 01117 // SPEED ADJUST METHOD 5 01118 // High speed when error is low, low speed when error is high 01119 // lower speed when more than third outside of center 01120 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.2; 01121 if (AbsError > ErrLimit) { 01122 LeftDriveRatio = MIN_POWER; 01123 } else { 01124 LeftDriveRatio = MAX_POWER; 01125 } 01126 RightDriveRatio = LeftDriveRatio; 01127 break; 01128 case 6: 01129 // SPEED ADJUST METHOD 6 01130 // High speed when error is low, low speed when error is high 01131 // lower speed when more than third outside of center 01132 if (AbsError > ABS_ERROR_THRESH) { 01133 LeftDriveRatio = MIN_POWER; 01134 } else { 01135 LeftDriveRatio = MAX_POWER; 01136 } 01137 RightDriveRatio = LeftDriveRatio; 01138 break; 01139 default: 01140 break; 01141 01142 } 01143 // TBD-- add speed adjust based on Xaccel sensor! 01144 01145 01146 // currently no control mechanism as don't have speed sensor 01147 CurrentLeftDriveSetting = (float) (LeftDriveRatio / 100) * MaxSpeed; 01148 CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed; 01149 01150 01151 if (terminalOutput == 3) { 01152 TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError); 01153 TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit); 01154 TERMINAL_PRINTF("MAX SPEED = %5.2f\n", MaxSpeed); 01155 TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting); 01156 TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting); 01157 } 01158 if (1 == 0) { 01159 TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting); 01160 TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting); 01161 } 01162 01163 } 01164 01165 void Drive() 01166 { 01167 01168 // START! 01169 // if not going, go when button A is pressed 01170 if (!go) { 01171 if(TFC_PUSH_BUTTON_0_PRESSED) { 01172 go = true; 01173 UnknownCount = 0; 01174 StartGateFoundCount = 0; 01175 startRaceTicker = TFC_Ticker[0]; // keep track of start of race 01176 logDataIndex = 0; // reset log data index 01177 } 01178 } 01179 01180 // STOP! 01181 // if going, stop when button A is pressed 01182 if (go) { 01183 if(TFC_PUSH_BUTTON_1_PRESSED) { 01184 go = false; 01185 StartGateFoundCount = 0; 01186 } 01187 } 01188 01189 // EMERGENCY STOP! 01190 // 'kill switch' to prevent crashes off-track 01191 if (killSwitch) { 01192 if (UnknownCount > UNKNOWN_COUNT_MAX) { // if track not found after certain time 01193 go = false; // kill engine 01194 StartGateFoundCount = 0; 01195 } 01196 } 01197 01198 // **************** 01199 01200 if (!go) { // stop! 01201 TFC_SetMotorPWM(0,0); //Make sure motors are off 01202 TFC_HBRIDGE_DISABLE; 01203 } 01204 01205 if (go) { // go! 01206 TFC_HBRIDGE_ENABLE; 01207 // motor A = right, motor B = left based on way it is mounted 01208 TFC_SetMotorPWM(CurrentRightDriveSetting,CurrentLeftDriveSetting); 01209 } 01210 } 01211 01212 01213 void adjustLights() 01214 { 01215 01216 // LIGHT ADJUST METHOD 1 01217 // threshold is 1/5 of light intensity 'range' 01218 if (1 == 0) { 01219 DerivThreshold = (float) (MaxLightIntensity - MinLightIntensity) / 5; 01220 NegDerivThreshold = (float) -1 * (DerivThreshold); 01221 PosDerivThreshold = (float) (DerivThreshold); 01222 } else { 01223 // LIGHT ADJUST METHOD 2 -- SEEMS TO WORK MUCH BETTER 01224 // pos edge threshold is half range of max deriv above aver derive 01225 // neg edge threshold is half range of min deriv above aver derive 01226 01227 NegDerivThreshold = (float) (minDerVal - aveDerVal) * DER_RATIO; 01228 PosDerivThreshold = (float) (maxDerVal - aveDerVal) * DER_RATIO; 01229 01230 } 01231 01232 printAdjustLightsData(); 01233 01234 } 01235 01236 void printAdjustLightsData() 01237 { 01238 if (terminalOutput == 3) { 01239 TERMINAL_PRINTF("Max Light Intensity: %4d\r\n", MaxLightIntensity); 01240 TERMINAL_PRINTF("Min Light Intensity: %4d\r\n", MinLightIntensity); 01241 TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", DerivThreshold); 01242 } 01243 01244 } 01245 01246 void feedbackLights() 01247 { 01248 switch (CurrentTrackStatus) 01249 { 01250 case LineFound: 01251 TFC_BAT_LED0_OFF; 01252 TFC_BAT_LED1_ON; 01253 TFC_BAT_LED2_ON; 01254 TFC_BAT_LED3_OFF; 01255 break; 01256 case StartGateFound: 01257 TFC_BAT_LED0_ON; 01258 TFC_BAT_LED1_OFF; 01259 TFC_BAT_LED2_OFF; 01260 TFC_BAT_LED3_ON; 01261 break; 01262 default: 01263 TFC_BAT_LED0_OFF; 01264 TFC_BAT_LED1_OFF; 01265 TFC_BAT_LED2_OFF; 01266 TFC_BAT_LED3_OFF; 01267 } 01268 01269 } 01270
Generated on Sat Jul 23 2022 09:49:35 by
1.7.2