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
Spices/Spices.cpp
- Committer:
- redxeth
- Date:
- 2014-04-20
- Revision:
- 0:98e98e01a6ce
File content as of revision 0:98e98e01a6ce:
#include "mbed.h" #include "Spices.h" #include "common.h" #include "TFC.h" // camera params #define NUM_LINE_SCAN 128 #define MAX_LINE_SCAN NUM_LINE_SCAN-1 #define MIN_LINE_WIDTH 0 #define MAX_LINE_WIDTH 15 #define FILTER_ENDS 0 // # of pixels at end of camera data to ignore; set to 0 for now, later make 15 #define RANGE (NUM_LINE_SCAN - (2 * FILTER_ENDS)) // range of camera pixels to consider #define ERR_RATIO 0.85 // ratio of max possible error to pixels (have to measure!) #define DER_RATIO 0.5 // ratio for der threshold level (was 0.5 initially, may put back) // steer/servo params #define MAX_STEER_LEFT -0.51 // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach) #define MAX_STEER_RIGHT 0.39 // value determined by demo mode 1 measure #define DT 0.02 // # MS of time between intervals (doesn't really matter) // logging parameters #define NUM_LOG_FRAMES 700 // # of frames to log (when logging active) ~14 sec worth! // ****** for debug tuning ****** #define TUNE_SPEED 0.7 #define TUNE_KP 0.008 #define TUNE_KI 0 #define TUNE_KD 0 #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 #define SPEED_ADJUST 4 #define ABS_ERROR_THRESH 10 // number of pixels line position offset before changing KP value #define CONTROL_METHOD 2 // which control method to use // Drive/motor params // 0.4 way too slow!! need to charge battery #define SUB_LIGHT_SPEED 0.5 // moderate speed (value 0 to 1 sent to motors) #define LIGHT_SPEED 0.6 // fast... #define RIDICULOUS_SPEED 0.7 // faster... #define LUDICROUS_SPEED 0.9 // faster still! #define MAX_POWER 100 // percent max power (for speed adjustments) // algo params #define UNKNOWN_COUNT_MAX 50 // max value to allow for unknown track conditions before killing engine #define STARTGATEFOUNDMAX 0 // max value to allow for finding starting gate before killing engine #define STARTGATEDELAY 50 // Delay before searching for starting gate to kill engine #define MMA8451_I2C_ADDRESS (0x1d<<1) // address for accelerometer? /* CAR INTERFACE DIP SWITCH: ----------------------------------------------------------------- 1 - ON: Run MCP below; OFF: Run Demo program (see main.cpp) 2 - ON: Log frame data to array 3 - ON: Risky race option; OFF: Conservative race option 4 - ON: Start Gate Kill Switch Active POTS ----------------------------------------------------------------- 0 - controls nothing at the moment 1 - controls nothing at the moment PUSHBUTTONS ----------------------------------------------------------------- A - START car race! B - END CAR RACE / (while holding down when 'log frame data' active will also output terminal data) */ // LEARNING CAR CLUB 9/10/13: // IP need to test-- get around U turns -- more aggressive proportional control? Or derivative? Have camera look farther ahead but not too far ahead // IP need to test -- fix lighting for tunnels (if good algo doesn't need lighting!!) // -- increase power up to get up hills, brake down hills (accel: http://mbed.org/users/SomeRandomBloke/code/MMA8451Q/#) // -- add speed control? (hall effect sensor) // DONE -- make sure steering won't go past limits!! // 9/12/13 - adjust camera exposure time based on maximum light intensity!! // 9/14/13 - DONE -- make derivative threshold related to maximum light intensity! // 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. // 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 // control instead... parabolic?? // DONE -- Also need to slow down on curves // speed up hills and slow down on downhill... // measure speed? // 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! // TODO: Need to increase rate at which camera sampled and decisions are made!! Look to codewarrior code // Need to cut off left/right ends of camera data-- seems to not read line properly in well lit rooms // DONE Speed adjust as you go round; Need to have it slow down "into" curve, speed up again "out of curve" // 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. // TODO: Need to see how often TFC_LineScanImageReady gets updated. If update camera sample freq how will that impact exposure? Light adjustment algos should // 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 // to calculate how fast servo needs to really go based on track curves and speed. // TODO: Need a way to measure speed I think as well. // TODO: Need to control speed differentially across the different motors!! See Eli video!! // 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) // TODO: Use PID control for steering! // TODO: Use Speed control (PID?) --- add speed sensor! // 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 // Need to figure out why derivative control in steering doesn't work well. Add integral control. // Latest track times (no hill, no bumps, no tunnel, only squiggles) = 11.8sec at high speed with speed control // Losing time on the curves-- need to optimize!! // Worry about hill later-- need to get track times down around 8sec first. // ***************************** // ** 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 // ** (U-TURNS take the longest time out of track) // ** Method to help find the line even when not really visible // ***************************** // 10/17/13 // NEED CURVE AND WIGGLE DETECT!! // 10/20/13 Added logging capability and 'algo time' detect // It is not finding the line on the edges when on a curve-- likely because my line detect algo requires both edges // TODO-- need method that can use only one edge! // TODO LIST // - Speed Control via Sensor WAITING ON SENSOR // - Differential drive around curves DONE -- still trying to figure out what %age to drop on turns // - Full PID steering control IN PROGRESS // - Starting gate kill engine debug IN PROGRESS -- need to add delay // - Off track kill engine debug -- IN PROGRESS-- seems to kill car prematurely // image processing vars uint16_t GrabLineScanImage0[NUM_LINE_SCAN]; // snapshot of camera data for this 'frame' float DerivLineScanImage0[NUM_LINE_SCAN]; // derivative of line scan data float NegEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data negative edges found float PosEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data positive edges found uint16_t numNegEdges = 0, numPosEdges = 0; // max value of valid neg and positive indices (also serves as a count of # edges found) uint16_t MaxLightIntensity = 0; // max measured light intensity -- to account for lighting differences uint16_t MinLightIntensity = (1 << 12); // min measured light intensity -- to account for lighting differences float maxDerVal = 0; // max deriv value float minDerVal = (float) (1 << 12); // min deriv value float aveDerVal = 0; // average deriv value float DerivThreshold = (1 << 9); // Derivative Threshold (default) float PosDerivThreshold = (1 << 9); // Pos Edge Derivative Threshold (default) float NegDerivThreshold = (1 << 9); // Neg Edge Derivative Threshold (default) // Steering control variables float CurrentLinePosition; // Current position of track line (in pixels -- 0 to 127) float LastLinePosition; // Last position of track line (in pixels -- 0 to 127) float CurrentLinePosError = 0; // Current line position error (used for derivative calc) float AbsError; float LastLinePosError = 0; // Last line position error (used for derivative calc) float SumLinePosError = 0; // Sum of line position error (used for integral calc) float DerivError = 0; // Derivative of error float CurrentSteerSetting = (MAX_STEER_RIGHT + MAX_STEER_LEFT) / 2; // drive straight at first float CurrentLeftDriveSetting = 0; // Drive setting (left wheel) float CurrentRightDriveSetting = 0; // Drive setting (right wheel) // Speed control vars float MaxSpeed; // maximum speed allowed uint16_t startRaceTicker; // ticker at start of race1 // Custom Data Types typedef enum TrackStatusType {Unknown, LineFound, StartGateFound, LineJustLeft} TrackStatusType; TrackStatusType CurrentTrackStatus; // current track status TrackStatusType LastTrackStatus; // last track status /* typedef enum TrackType {NotSure, Straight, Curve, Wiggle, Bumps, StartGate, UpHill, DownHill} TrackType; TrackType CurrentTrack; */ struct LogData { float linepos; float steersetting; float leftdrivesetting; float rightdrivesetting; }; LogData frameLogs[NUM_LOG_FRAMES]; // array of log data to store int logDataIndex; // index for log data int StartGateFoundCount = 0; // how many times start gate has been found int UnknownCount = 0; // how many times nothing has been found (to help with kill switch implementation) bool go = false; // Car can go! Should be set to false to start. // EXTRA CONTROL PARAMETERS bool debugFakeMode = false; // if true, ignores real camera and uses fake camera input instead; used for data processing debug int terminalOutput = 0; // set debug level for terminal output // 0 : no terminal output, race! // 1 : output just to measure frame rate // 2 : output for measuring time of operations // 3 : output with delay bool doLogData = false; // whether to capture log data to output later on bool killSwitch = false; // whether to enable Kill Switch (allow engine to stop after not finding track) bool startGateStop = false; // whether to stop or not depending on starting gate reading bool doRisky = false; // race style-- whether conservative or risky // timer stuff Timer timer; int after_time, before_time, start_time, last_start_time; bool run_once = false; void MasterControlProgram() { // put here all things want to run only once after reset if (!run_once){ if ((terminalOutput == 1) || (terminalOutput == 2)){ timer.start(); } run_once = true; } // read DIP switches and Pots for data readSwitches(); // Every 4s (or Terminal Output is off, i.e. race mode!) // AND line scan image ready (or fake mode where image is always ready) // (ticker updates every 2ms) (Line scan image ready very 20mS?) if((TFC_Ticker[0]>2000 || (terminalOutput != 3)) && (TFC_LineScanImageReady>0 || debugFakeMode)) { // stuff that needs to be reset with each image frame if (terminalOutput == 1) { last_start_time = start_time; start_time = timer.read_us(); TERMINAL_PRINTF("TIME:Between frames:%d:uSec\r\n", start_time - last_start_time); before_time = timer.read_us(); } TFC_Ticker[0] = 0; TFC_LineScanImageReady=0; // must reset to 0 after detecting non-zero MaxLightIntensity = 0; // reset MinLightIntensity = (1 << 12); // reset // grab camera frame grabCameraFrame(); if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME:TO AFTER grabCameraFrame:%d:uSec\r\n", after_time - before_time); before_time = timer.read_us(); } // calcalate derivative of linescandata, filter starttime data derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]); if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME:TO AFTER derivativeLineScan:%d:uSec\r\n", after_time - before_time); before_time = timer.read_us(); } // adjust deriv threshold based on max lighting value // has to be called before find edges adjustLights(); if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME:TO AFTER adjustLights:%d:uSec\r\n", after_time - before_time); before_time = timer.read_us(); } //find edges from derivative data findEdges_v2(&DerivLineScanImage0[0]); if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME:TO AFTER findEdges_v2:%d:uSec\r\n", after_time - before_time); before_time = timer.read_us(); } // turn on terminal output if line not found -- FOR DEBUG //if (CurrentTrackStatus == Unknown) // terminalOutput = 1; //review edge data and set position or starting flag appropriately reviewEdges(); if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME:TO AFTER reviewEdges:%d:uSec\r\n", after_time - before_time); before_time = timer.read_us(); } if (terminalOutput == 3) { // print data to Terminal for camera 0 printLineScanData(&GrabLineScanImage0[0]); // print deriviative of linescandata, filter starttime data printDerivLineScanData(&DerivLineScanImage0[0]); printAdjustLightsData(); printEdgesFound(); } // ** Track Status available at this point ** // test out accelerometer // accelTest(); // Update things based on latest track status // e.g. change steering setting, stop car, ... ActOnTrackStatus(); if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME:TO AFTER ActOnTrackStatus:%d:uSec\r\n", after_time - before_time); before_time = timer.read_us(); } //give LED feedback as to track status feedbackLights(); if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME:TO AFTER feedbackLights:%d:uSec\r\n", after_time - before_time); before_time = timer.read_us(); } // control max power (speed) SpeedControl(); if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME:TO AFTER SpeedControl:%d:uSec\r\n", after_time - before_time); before_time = timer.read_us(); } // Drive!! Drive(); if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME:TO AFTER Drive:%d:uSec\r\n", after_time - before_time); before_time = timer.read_us(); } // wait_ms(1); // Capture Log data while driving if (go && doLogData) { captureData(); } // Dump Log data to Terminal while stopped and holding B button if (!go && doLogData && TFC_PUSH_BUTTON_1_PRESSED) { dumpData(); } if (terminalOutput == 2) { after_time = timer.read_us(); TERMINAL_PRINTF("TIME: ENTIRE FRAME (include prints):%d:uSec\r\n", after_time - start_time); before_time = timer.read_us(); } if (terminalOutput == 3) { TERMINAL_PRINTF("\r\n**************************END********************************\r\n"); } } } void dumpData() { TERMINAL_PRINTF("INDEX,LINEPOS,STEERSETTING,LEFTDRIVESETTING,RIGHTDRIVESETTING\r\n"); for(logDataIndex=0;logDataIndex<NUM_LOG_FRAMES;logDataIndex++) { 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); } } void captureData() { frameLogs[logDataIndex].linepos = CurrentLinePosition; frameLogs[logDataIndex].steersetting = CurrentSteerSetting; frameLogs[logDataIndex].leftdrivesetting = CurrentLeftDriveSetting; frameLogs[logDataIndex].rightdrivesetting = CurrentRightDriveSetting; // increment index logDataIndex++; if (logDataIndex > NUM_LOG_FRAMES) logDataIndex = 0; } void readSwitches() { // ********* GATHER DIP SWITCH INPUTS ********* if(TFC_GetDIP_Switch()&0x02) // SWITCH 2 doLogData = true; // Log data to array else doLogData = false; // normal operation if(TFC_GetDIP_Switch()&0x04) // SWITCH 3 doRisky = true; else doRisky = false; if(TFC_GetDIP_Switch()&0x08) // SWITCH 4 control start stop gate startGateStop = true; else startGateStop = false; } void grabCameraFrame() { uint32_t i = 0; uint8_t fake_type = 4; // type of fake data if used for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0 { if (debugFakeMode) { // use fake camera data switch (fake_type) { case 0: // ideal track -- line in center if (i<57 || i > 70) GrabLineScanImage0[i] = 0xFFF; // no line else GrabLineScanImage0[i] = 0x4B0; // line break; case 1: // ideal track -- line to the left if (i<27 || i > 40) GrabLineScanImage0[i] = 0xFFF; // no line else GrabLineScanImage0[i] = 0x4B0; // line break; case 2: // ideal track -- line to the right if (i<87 || i > 100) GrabLineScanImage0[i] = 0xFFF; // no line else GrabLineScanImage0[i] = 0x4B0; // line break; case 3: // ideal track -- starting gate! // TBD break; case 4: // less than ideal track -- debug multi-edge issue! if (i<54) GrabLineScanImage0[i] = 4000; // no line if (i == 54) GrabLineScanImage0[i] = 3370; // neg edge if (i == 55) GrabLineScanImage0[i] = 3309; // neg edge if (i == 56) GrabLineScanImage0[i] = 2016; // neg edge if (i == 57) GrabLineScanImage0[i] = 711; // neg edge if (i == 58) GrabLineScanImage0[i] = 696; // neg edge if ((i>58) && (i<69)) GrabLineScanImage0[i] = 500; // line if (i == 69) GrabLineScanImage0[i] = 1800; // pos edge if (i > 69) GrabLineScanImage0[i] = 4000; // no line default: break; } } else { // use real camera data GrabLineScanImage0[i] = TFC_LineScanImage0[i]; } } } void printLineScanData(uint16_t* LineScanData) { uint32_t i = 0; float Val; TERMINAL_PRINTF("LINE SCAN DATA:,"); for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0 { if (1 == 1) { // use float to print Val = (float) LineScanData[i]; TERMINAL_PRINTF("%9.3f",Val); if(i==MAX_LINE_SCAN) // when last data reached put in line return TERMINAL_PRINTF("\r\n"); else TERMINAL_PRINTF(","); } else { TERMINAL_PRINTF("0x%X",LineScanData[i]); if(i==MAX_LINE_SCAN) // when last data reached put in line return TERMINAL_PRINTF("\r\n",LineScanData[i]); else TERMINAL_PRINTF(",",LineScanData[i]); } } } void printDerivLineScanData(float* derivLineScanData) { uint32_t i, minCnt = 0, maxCnt = 0; minCnt = FILTER_ENDS; maxCnt = NUM_LINE_SCAN - FILTER_ENDS; TERMINAL_PRINTF("DERIVATIVE DATA:,"); for(i=minCnt;i<maxCnt;i++) // print one line worth of data (128) from Camera 0 { TERMINAL_PRINTF("%9.3f",derivLineScanData[i]); if(i==maxCnt-1) // when last data reached put in line return TERMINAL_PRINTF("\r\n",derivLineScanData[i]); else TERMINAL_PRINTF(", ",derivLineScanData[i]); } } void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut) { uint32_t i, minCnt = 0, maxCnt = 0; float DerVal, upperDerVal, lowerDerVal = 0; maxDerVal = 0; minDerVal = (float) (1 << 12); aveDerVal = 0; minCnt = FILTER_ENDS; maxCnt = NUM_LINE_SCAN - FILTER_ENDS; // TERMINAL_PRINTF("i, upperDerVal, lowerDerVal, DerVal\r\n"); for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 { // store max light intensity value if (LineScanDataIn[i] > MaxLightIntensity) MaxLightIntensity = LineScanDataIn[i]; // store min light intensity value if (LineScanDataIn[i] < MinLightIntensity) MinLightIntensity = LineScanDataIn[i]; // Central Derivative if (i==minCnt) { // start point upperDerVal = (float)(LineScanDataIn[i+1]); lowerDerVal = (float)(LineScanDataIn[i]); // make same as start point } else if (i==maxCnt - 1){ // end point upperDerVal = (float)(LineScanDataIn[i]); // make same as end point lowerDerVal = (float)(LineScanDataIn[i-1]); } else { // any other point upperDerVal = (float)(LineScanDataIn[i+1]); lowerDerVal = (float)(LineScanDataIn[i-1]); } DerVal = (upperDerVal - lowerDerVal) / 2; // TERMINAL_PRINTF("%d,%9.3f,%9.3f,%9.3f\r\n", i, upperDerVal, lowerDerVal, DerVal); if (DerVal > maxDerVal) { maxDerVal = DerVal; } if (DerVal < minDerVal) { minDerVal = DerVal; } aveDerVal = aveDerVal + DerVal; //get sum DerivLineScanDataOut[i] = DerVal; } aveDerVal = (float) aveDerVal / (maxCnt - minCnt); } //Not reliable for finding edges! void findEdges(float* derivLineScanData) { // search for edges in deriviative data using a threshold // need to store in a hash if that's possible... // combine edges that are a pixel apart int i, minCnt = 0, maxCnt = 0; int multiNegEdgeCnt = 1, multiNegEdgeSum = 0; int multiPosEdgeCnt = 1, multiPosEdgeSum = 0; minCnt = FILTER_ENDS; maxCnt = NUM_LINE_SCAN - FILTER_ENDS; numNegEdges = 0; numPosEdges = 0; for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 { if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND! { if (terminalOutput == 3) { TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); } if ((numNegEdges > 0) && (NegEdges[numNegEdges - 1] + 1 == i )) // if no multi edges { // edge actually across multiple pixels multiNegEdgeCnt++; multiNegEdgeSum = multiNegEdgeSum + i; if (terminalOutput == 3) { TERMINAL_PRINTF("MULTIEDGE FOUND! MultiNegEdgeCnt: %d; MultiNegEdgeSum: %d\r\n", multiNegEdgeCnt, multiNegEdgeSum); } } else { // not a multi-pixel edge known at this time, store negative edge index value numNegEdges++; if (terminalOutput == 3) { TERMINAL_PRINTF("NEG EDGE STORED WITH INDEX %d. NUM NEG EDGES = %d\r\n", i, numNegEdges); } NegEdges[numNegEdges - 1] = (float) i; multiNegEdgeSum = i; } } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND! if (terminalOutput == 3) { TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); } if ((numPosEdges > 0) && (PosEdges[numPosEdges - 1] + 1 == i )) { // edge actually across multiple pixels multiPosEdgeCnt++; multiPosEdgeSum = multiPosEdgeSum + i; if (terminalOutput == 3) { TERMINAL_PRINTF("MULTIEDGE FOUND! MultiPosEdgeCnt: %d; MultiPosEdgeSum: %d\r\n", multiPosEdgeCnt, multiPosEdgeSum); } } else { // not a multi-pixel edge known at this time, store Posative edge index value if (terminalOutput == 3) { TERMINAL_PRINTF("POS EDGE STORED WITH INDEX %d. NUM POS EDGES = %d\r\n", i, numPosEdges); } numPosEdges++; PosEdges[numPosEdges - 1] = (float) i; multiPosEdgeSum = i; } } else { // NO EDGE FOUND // combine multi-edges if there are any if (multiNegEdgeCnt > 1) { NegEdges[numNegEdges - 1] = (float) multiNegEdgeSum / multiNegEdgeCnt; multiNegEdgeCnt = 1; multiNegEdgeSum = 0; } if (multiPosEdgeCnt > 1) { PosEdges[numPosEdges - 1] = (float) multiPosEdgeSum / multiPosEdgeCnt; multiPosEdgeCnt = 1; multiPosEdgeSum = 0; } } } } void findEdges_v2(float* derivLineScanData) { // search for edges in deriviative data using a threshold // need to store in a hash if that's possible... // combine edges that are a pixel apart int i; int NegEdgeBufCnt = 0, NegEdgeBufSum = 0; // serves as buffer to store neg edges found next to each other int PosEdgeBufCnt = 0, PosEdgeBufSum = 0; // serves as buffer to store pos edges found next to each other int minCnt = FILTER_ENDS; int maxCnt = NUM_LINE_SCAN - FILTER_ENDS; numNegEdges = 0; // count of neg edges found thus far numPosEdges = 0; // count of pos edges found thus far for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 { if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND! { if (terminalOutput == 3) { TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); } NegEdgeBufCnt++; // add value to neg edge buffer NegEdgeBufSum = NegEdgeBufSum + i; } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND! if (terminalOutput == 3) { TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); } PosEdgeBufCnt++; // add value to pos edge buffer PosEdgeBufSum = PosEdgeBufSum + i; } else { // NO EDGE FOUND // POP EDGE BUFFERS IF NON-EMPTY AND STORE TO EDGE "STACK" (i.e. edges found) if (NegEdgeBufCnt > 0) { // store edge value numNegEdges++; NegEdges[numNegEdges - 1] = (float) NegEdgeBufSum / NegEdgeBufCnt; // clear edge buffer NegEdgeBufSum = 0; NegEdgeBufCnt = 0; } if (PosEdgeBufCnt > 0) { // store edge value numPosEdges++; PosEdges[numPosEdges - 1] = (float) PosEdgeBufSum / PosEdgeBufCnt; // clear edge buffer PosEdgeBufSum = 0; PosEdgeBufCnt = 0; } } } } void printEdgesFound() { int i; // Check that neg edges captured ok TERMINAL_PRINTF("NEGATIVE EDGES FOUND:,"); for(i=0;i<=numNegEdges-1;i++) { TERMINAL_PRINTF("%9.3f",NegEdges[i]); if(i==numNegEdges-1) // when last data reached put in line return TERMINAL_PRINTF("\r\n"); else TERMINAL_PRINTF(", "); } // Check that pos edges captured ok TERMINAL_PRINTF("POSITIVE EDGES FOUND:,"); for(i=0;i<=numPosEdges-1;i++) { TERMINAL_PRINTF("%9.3f",PosEdges[i]); if(i==numPosEdges-1) // when last data reached put in line return TERMINAL_PRINTF("\r\n"); else TERMINAL_PRINTF(", "); } } void reviewEdges() { LastTrackStatus = CurrentTrackStatus; if ((numPosEdges == 1) && (numNegEdges == 1)) // only one negative and positive edge found (LINE) { if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) // has proper expected width { CurrentTrackStatus = LineFound; // report line found! UnknownCount = 0; // reset unknown status count LastLinePosition = CurrentLinePosition; CurrentLinePosition = (PosEdges[0]+NegEdges[0]) / 2; // update line position } } else if ((numPosEdges == 1) && (numNegEdges == 0)) { // 1 pos edge found (POSSIBLE LINE) if ((PosEdges[0] <= MAX_LINE_WIDTH) && (LastLinePosError < 0)) // pos edge is within line width of edge of camera (LEFT) { CurrentTrackStatus = LineFound; // report line found! UnknownCount = 0; // reset unknown status count LastLinePosition = CurrentLinePosition; CurrentLinePosition = PosEdges[0] - ( MAX_LINE_WIDTH / 2); // update line position // TERMINAL_PRINTF("*** SINGLE POSEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); } } else if ((numNegEdges == 1) && (numPosEdges == 0)) { // 1 neg edge found (POSSIBLE LINE) if ((NegEdges[0] >= (MAX_LINE_SCAN - MAX_LINE_WIDTH)) && (LastLinePosError > 0)) // neg edge is within line width of edge of camera (RIGHT) { CurrentTrackStatus = LineFound; // report line found! UnknownCount = 0; // reset unknown status count LastLinePosition = CurrentLinePosition; CurrentLinePosition = NegEdges[0] + ( MAX_LINE_WIDTH / 2); // update line position // TERMINAL_PRINTF("*** SINGLE NEGEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); } } else if ((numPosEdges == 2) && (numNegEdges == 2)) { // 2 negative and 2 positive edges found (STARTING/FINISH GATE) if ((((NegEdges[0] - PosEdges[0]) >= MIN_LINE_WIDTH) && ((NegEdges[0] - PosEdges[0]) <= MAX_LINE_WIDTH)) && // white left 'line' (((NegEdges[1] - PosEdges[1]) >= MIN_LINE_WIDTH) && ((NegEdges[1] - PosEdges[1]) <= MAX_LINE_WIDTH)) && // white right 'line' (((PosEdges[1] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH)) // actual track line ) if (startRaceTicker > STARTGATEDELAY) { // only start counting for starting gate until after delay StartGateFoundCount++; } CurrentTrackStatus = StartGateFound; UnknownCount = 0; // reset unknown status count } else if ((numPosEdges > 1) && (numNegEdges > 1)) { // more than 1 negative edge and positive edge found (but not 2 for both) (STARTING / FINISH GATE) // remove edges that aren't close to center TBD DDHH if (terminalOutput == 3) { TERMINAL_PRINTF("***************************************** \r\n"); TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n"); TERMINAL_PRINTF("***************************************** \r\n"); } CurrentTrackStatus = Unknown; } else { // no track or starting gate found if (terminalOutput == 3) { TERMINAL_PRINTF("***************************************** \r\n"); TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n", CurrentLinePosition); TERMINAL_PRINTF("***************************************** \r\n"); } CurrentTrackStatus = Unknown; UnknownCount++; } } void ActOnTrackStatus() { // Decide what to do next based on current track status if (CurrentTrackStatus == LineFound) { // LINE FOUND! if (terminalOutput == 3) { TERMINAL_PRINTF("***************************************** \r\n"); TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); TERMINAL_PRINTF("***************************************** \r\n"); } // Update steering position SteeringControl(); // Apply to servo Steer(); } else if (CurrentTrackStatus == StartGateFound) { // STARTING GATE FOUND if (terminalOutput == 3) { TERMINAL_PRINTF("***************************************** \r\n"); TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n"); TERMINAL_PRINTF("********** count = %d ********** \r\n", StartGateFoundCount); TERMINAL_PRINTF("***************************************** \r\n"); } // END RACE! if (startGateStop) { if (StartGateFoundCount > STARTGATEFOUNDMAX) { go = false; // STOP!! } } } } void SteeringControl() { float targetPosition = (float)( (NUM_LINE_SCAN / 2) - 0.5); // target to achieve for line position float KP; // proportional control factor float KI; // integral control factor float KD; // derivative control factor float Pout, Iout, Dout; // PID terms // Calculate error // make error to the right positive // i.e. if LINE to the right-- then CurrentLinePosError > 0 // if LINE to the left -- then CurrentLinePosError < 0 CurrentLinePosError = CurrentLinePosition - targetPosition; // Get absolute error if (CurrentLinePosError >= 0) AbsError = CurrentLinePosError; else AbsError = -1 * CurrentLinePosError; // CHOOSE SET OF PID CONTROL PARAMETERS switch (CONTROL_METHOD) { case 0: // Pure proportional control based on range of steering values vs. range of error values KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH ); KD = 0; KI = 0; break; case 1: // Proportional control with 50% bit more oomph --- a bit more aggressive around the bends KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH ); KP = KP * 1.5; KD = 0; KI = 0; break; case 2: // MANUAL TUNING CASE 1 (use pot to help determine tuning parameters) KP = TUNE_KP; KI = TUNE_KI; KD = TUNE_KD; case 3: if (AbsError < ABS_ERROR_THRESH) { KP = 0.003; // when relatively straight, keep KP gain low } else { KP = 0.010; // when curve begins or off track, increase KP gain } KI = 0; KD = 0; default: break; } /* Pseudocode previous_error = 0 integral = 0 start: error = setpoint - measured_value integral = integral + error*dt derivative = (error - previous_error)/dt output = Kp*error + Ki*integral + Kd*derivative previous_error = error wait(dt) goto start */ if (terminalOutput == 3) { TERMINAL_PRINTF("KP = %6.4f\r\n", KP); TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition); } // Update integral of error // i.e. if LINE stays to the right, then SumLinePosError increases // i.e. if LINE stays to the left, then SumLinePosError decreases SumLinePosError = SumLinePosError + ( CurrentLinePosError * DT ); DerivError = (CurrentLinePosError - LastLinePosError) / DT; if (terminalOutput == 3) { TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n", CurrentLinePosition); TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n", CurrentLinePosError); } // SECOND- calculate new servo position // proportional control term Pout = KP * CurrentLinePosError; // integral control term Iout = KI * SumLinePosError; // Derivative control term Dout = KD * DerivError; if (terminalOutput == 3) { TERMINAL_PRINTF("KP = %6.4f\r\n", KP); TERMINAL_PRINTF("KI = %6.4f\r\n", KI); TERMINAL_PRINTF("KD = %6.4f\r\n", KD); TERMINAL_PRINTF("Pout = %6.4f\r\n", Pout); TERMINAL_PRINTF("Iout = %6.4f\r\n", Iout); TERMINAL_PRINTF("Dout = %6.4f\r\n", Dout); } // Finally add offset to steering to account for non-centered servo mounting // CurrentSteerSetting = Pout + Iout + Dout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ); CurrentSteerSetting = Pout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ); // store for next cycle deriv calculation LastLinePosError = CurrentLinePosError; // for tuning control algo only if (1 == 0) { TERMINAL_PRINTF("*** ******************************** \r\n"); TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); TERMINAL_PRINTF("*** ERROR %9.3f *** \r\n", CurrentLinePosError); TERMINAL_PRINTF("*** INTEGRAL ERROR %9.3f *** \r\n", SumLinePosError); TERMINAL_PRINTF("*** DERIVATIVE ERROR %9.3f *** \r\n", DerivError); TERMINAL_PRINTF("*** P STEER SETTING %9.3f *** \r\n", CurrentSteerSetting); TERMINAL_PRINTF("*** PI STEER SETTING %9.3f *** \r\n", (CurrentSteerSetting + Iout)); TERMINAL_PRINTF("*** ******************************** \r\n"); wait_ms(1000); } } void Steer() { // make sure doesn't go beyond steering limits if (CurrentSteerSetting > MAX_STEER_RIGHT) { CurrentSteerSetting = MAX_STEER_RIGHT; } else if (CurrentSteerSetting < MAX_STEER_LEFT) { CurrentSteerSetting = MAX_STEER_LEFT; } if (terminalOutput == 3) { TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n", CurrentSteerSetting); } TFC_SetServo(0,CurrentSteerSetting); } void SpeedControl() { // Get max speed setting from reading pot0 // then adjust float ErrLimit; float LeftDriveRatio, RightDriveRatio; // set maximum speed allowed switch (1) { case 0: // read value off pot0 MaxSpeed = TFC_ReadPot(0); break; case 1: if (doRisky) MaxSpeed = TUNE_SPEED + 0.1; else MaxSpeed = TUNE_SPEED; break; case 2: MaxSpeed = SUB_LIGHT_SPEED; break; case 3: MaxSpeed = LIGHT_SPEED; break; case 4: MaxSpeed = RIDICULOUS_SPEED; break; case 5: MaxSpeed = LUDICROUS_SPEED; break; default: break; } switch (SPEED_ADJUST) { case 0: // SPEED ADJUST METHOD 0 // no speed adjust LeftDriveRatio = MAX_POWER; RightDriveRatio = LeftDriveRatio; case 1: // SPEED ADJUST METHOD 1 // High speed when error is low, low speed when error is high // lower speed when more than third outside of center ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.33; if (AbsError > ErrLimit) { LeftDriveRatio = MIN_POWER; } else { LeftDriveRatio = MAX_POWER; } RightDriveRatio = LeftDriveRatio; break; case 2: // SPEED ADJUST METHOD 2 // Have max/min speed adjust proportional to absolute value of line error ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO; LeftDriveRatio = MAX_POWER - ((MAX_POWER - MIN_POWER) * (AbsError / ErrLimit)); RightDriveRatio = LeftDriveRatio; break; case 3: // SPEED ADJUST METHOD 3 // have wheel relative speed proportional to absolute value of line error ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO; if (CurrentLinePosError > 0) { // heading right LeftDriveRatio = MAX_POWER; RightDriveRatio = (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER; } else if (CurrentLinePosError < 0) { // heading left RightDriveRatio = MAX_POWER; LeftDriveRatio = (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER; } else { LeftDriveRatio = MAX_POWER; RightDriveRatio = MAX_POWER; } break; case 4: // SPEED ADJUST METHOD 4 // have wheel relative speed proportional to absolute value of line error // only when above a certain error ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.1; if (CurrentLinePosError > ErrLimit) { // heading right LeftDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); RightDriveRatio = MIN_POWER; } else if (CurrentLinePosError < (-1 * ErrLimit)) { // heading left RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); LeftDriveRatio = MIN_POWER; } else { LeftDriveRatio = MAX_POWER; RightDriveRatio = MAX_POWER; } break; case 5: // SPEED ADJUST METHOD 5 // High speed when error is low, low speed when error is high // lower speed when more than third outside of center ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.2; if (AbsError > ErrLimit) { LeftDriveRatio = MIN_POWER; } else { LeftDriveRatio = MAX_POWER; } RightDriveRatio = LeftDriveRatio; break; case 6: // SPEED ADJUST METHOD 6 // High speed when error is low, low speed when error is high // lower speed when more than third outside of center if (AbsError > ABS_ERROR_THRESH) { LeftDriveRatio = MIN_POWER; } else { LeftDriveRatio = MAX_POWER; } RightDriveRatio = LeftDriveRatio; break; default: break; } // TBD-- add speed adjust based on Xaccel sensor! // currently no control mechanism as don't have speed sensor CurrentLeftDriveSetting = (float) (LeftDriveRatio / 100) * MaxSpeed; CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed; if (terminalOutput == 3) { TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError); TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit); TERMINAL_PRINTF("MAX SPEED = %5.2f\n", MaxSpeed); TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting); TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting); } if (1 == 0) { TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting); TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting); } } void Drive() { // START! // if not going, go when button A is pressed if (!go) { if(TFC_PUSH_BUTTON_0_PRESSED) { go = true; UnknownCount = 0; StartGateFoundCount = 0; startRaceTicker = TFC_Ticker[0]; // keep track of start of race logDataIndex = 0; // reset log data index } } // STOP! // if going, stop when button A is pressed if (go) { if(TFC_PUSH_BUTTON_1_PRESSED) { go = false; StartGateFoundCount = 0; } } // EMERGENCY STOP! // 'kill switch' to prevent crashes off-track if (killSwitch) { if (UnknownCount > UNKNOWN_COUNT_MAX) { // if track not found after certain time go = false; // kill engine StartGateFoundCount = 0; } } // **************** if (!go) { // stop! TFC_SetMotorPWM(0,0); //Make sure motors are off TFC_HBRIDGE_DISABLE; } if (go) { // go! TFC_HBRIDGE_ENABLE; // motor A = right, motor B = left based on way it is mounted TFC_SetMotorPWM(CurrentRightDriveSetting,CurrentLeftDriveSetting); } } void adjustLights() { // LIGHT ADJUST METHOD 1 // threshold is 1/5 of light intensity 'range' if (1 == 0) { DerivThreshold = (float) (MaxLightIntensity - MinLightIntensity) / 5; NegDerivThreshold = (float) -1 * (DerivThreshold); PosDerivThreshold = (float) (DerivThreshold); } else { // LIGHT ADJUST METHOD 2 -- SEEMS TO WORK MUCH BETTER // pos edge threshold is half range of max deriv above aver derive // neg edge threshold is half range of min deriv above aver derive NegDerivThreshold = (float) (minDerVal - aveDerVal) * DER_RATIO; PosDerivThreshold = (float) (maxDerVal - aveDerVal) * DER_RATIO; } printAdjustLightsData(); } void printAdjustLightsData() { if (terminalOutput == 3) { TERMINAL_PRINTF("Max Light Intensity: %4d\r\n", MaxLightIntensity); TERMINAL_PRINTF("Min Light Intensity: %4d\r\n", MinLightIntensity); TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", DerivThreshold); } } void feedbackLights() { switch (CurrentTrackStatus) { case LineFound: TFC_BAT_LED0_OFF; TFC_BAT_LED1_ON; TFC_BAT_LED2_ON; TFC_BAT_LED3_OFF; break; case StartGateFound: TFC_BAT_LED0_ON; TFC_BAT_LED1_OFF; TFC_BAT_LED2_OFF; TFC_BAT_LED3_ON; break; default: TFC_BAT_LED0_OFF; TFC_BAT_LED1_OFF; TFC_BAT_LED2_OFF; TFC_BAT_LED3_OFF; } }