This is an example program that actually allows the car to race using the FRDM-TFC library!
Fork of TFC-RACING-DEMO by
Overview
This is an example program that uses the FRDM-TFC library to actually permit a car using the FDRM-TFC shield + FRDM-KL25Z to race around a center line track.
Exercises designed for Mentoring Matters Car Summer Camp for Summer 2014 at Freescale, Inc. in Austin, Texas
Car MODES
5 MODES OR EXERCISES THAT WILL ALIGN WITH DIP SWITCH SETTINGS IN BINARY FASHION e.g. Mode 1 = 001 = switch 1 is on, switch 2 is off, switch 3 is off
0 = 000 = Garage Mode, button light test to see if car alive!!
- PUSHBUTTON A - Light LEDs 0 and 1
- PUSHBUTTON B - Light LEDs 2 and 3
- (switch 4 does nothing)
1 = 001 = Garage Mode, forward/reverse adjust, no auto steer, terminal output
- POT 1 - Controls speed of right wheel: CCW = reverse; CW = forward
- POT 2 - Controls speed of left wheel: CCW = reverse; CW = forward
- NOTE In this mode the high pitched whine heard is of the H-bridge being active. To disable whine, briefly put into demo mode 1 above.
- (switch 4 does nothing)
2 = 010 = Garage Mode, steering adjust, no auto steer, terminal output
- POT 1 - Controls Servo: CCW = left; CW = right
- (switch 4 does nothing)
3 = 011 = Garage Mode, Camera test, some auto steer, terminal output
- switch 4 : OFF = normal dec data, ON = o-scope mode
4 = 100 = Track Mode, Auto Steer, safe settings
- LIGHT SPEED = 0.4 default
- switch 4 = terminal output on/off
- Pot0 = controls motor speed
- Pot1 = controls value to multiply with Kp for proportional control
5 = 101 = Track Mode, Auto Steer, fast settings
- LUDICROUS SPEED = 0.6 default
- switch 4 = terminal output on/off
- Pot0 = controls motor speed
- Pot1 = controls value to multiply with Kp for proportional control
6 = 110 = future upgrades
7 = 111 = future upgrades
Car Hookup
Motors
- Code written assuming left motor hooked to B1(red)/B2(black) and right motor hooked to A1(red)/A2(black).
Battery
- Be sure to hook positive (red) to 'Vbat' and negative (black) to 'Gnd'
Steering Servo
- Servo must be hooked up with black wire innermost (away from LEDs).
- Also be sure to mount servo on chassis with wire coming out the right side of the car.
Camera
- Camera must be hooked up with black wire towards LEDs on the shield board.
- Be sure to mount camera on tower with connector down towards the bottom.
Potentiometers (Pots)
- Pots by default should have arrows pointing toward battery/motor hookup (for demo mods default).
Car Calibration
Serial Terminal
- Download your favorite Terminal application (I use TeraTerm. Set it for 115200 baud, 8bit, none, 1bit.
- But first you have to be sure that Windows mbed serial driver has been installed: Windows serial config.
Camera
- Be sure to focus the camera properly. See demo video.
- Be sure to prevent light noise on camera: Avoid Light Noise
- More camera info
Servo/Steering
- You will need to put the car into demo mode 1 and connect up a terminal to the serial port in order to get feedback on the values for your particular servo as hooked up. Then change
MAX_STEER_LEFT
andMAX_STEER_RIGHT
inSpices.cpp
with these values to tell the program how your servo is hooked up.
Speed Control
- This program does not have proper speed control but does modify the speed slightly based on recent error results from the camera. It also modifies the differential speed to each wheel to have better control around curves.
- While debugging your car you may want to lower the speed. See the constants
LIGHT_SPEED
andLUDICROUS_SPEED
inSpices.cpp
. There you can change the speeds used for Modes 4 and 5 above. Range of valid values are 0.4-0.7. Lower is better when debugging the car around the track (mainly to minimize crash forces!).
Strange Gotchas
Glitchy Motors
- Apparently there is contention between TPM0_CH0 and OpenSDA micro that causes strange issues with Motors (PWM interference). This will cause glitches in motor activty when hooked up to USB only: Found contention
More Info
- The Freescale Cup main info page
- redxeth youtube channel with useful related videos\
Spices/Spices.cpp
- Committer:
- simonchow
- Date:
- 2014-06-25
- Revision:
- 2:999ca57934bf
- Parent:
- 1:87b28e8b9941
File content as of revision 2:999ca57934bf:
#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.42 // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach) #define MAX_STEER_RIGHT 0.43 // 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_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 // do not change #define ABS_ERROR_THRESH 10 // number of pixels line position offset before changing KP value #define CONTROL_METHOD 1 // which control method to use // Drive/motor params #define LIGHT_SPEED 0.4 // easy speed #define LUDICROUS_SPEED 0.6 // faster speed #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 // 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 bool terminalOutput = false; // whether output terminal data 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 void TrackMode() { // set mode based on DIP switches useMode(); // grab Terminal Mode based on DIP switch 4 terminalOutput = terminalMode(); // Every 2s (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 10ms) (Line scan image ready every 10ms) if((TFC_Ticker[0]>2000 || (!terminalOutput)) && (TFC_LineScanImageReady>0 || debugFakeMode)) { // stuff that needs to be reset with each image frame 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(); // calcalate derivative of linescandata, filter starttime data derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]); // adjust deriv threshold based on max lighting value // has to be called before find edges adjustLights(); //find edges from derivative data findEdges_v2(&DerivLineScanImage0[0]); //review edge data and set position or starting flag appropriately reviewEdges(); // print terminal data if desired (slows down things) if (terminalOutput) { printLineScanData(&GrabLineScanImage0[0]); printDerivLineScanData(&DerivLineScanImage0[0]); printAdjustLightsData(); printEdgesFound(); } // ** Track Status available at this point ** // Update things based on latest track status // e.g. change steering setting, stop car, ... ActOnTrackStatus(); //give LED feedback as to track status feedbackLights(); // control max power (speed) SpeedControl(); // Drive!! Drive(); if (terminalOutput) { TERMINAL_PRINTF("\r\n**************************END********************************\r\n"); } } } uint16_t getMode(){ // get mode based on first 3 DIP switches (1, 2, 3) return (TFC_GetDIP_Switch()&0x07); } void useMode() { switch(getMode()) { default: case 4 : // Track Mode, Auto Steer, safe settings doRisky = false; break; case 5 : // Track Mode, Auto Steer, fast settings doRisky = true; break; case 6 : // NOT USED YET break; case 7 : // NOT USED YET break; } // end case if(TFC_GetDIP_Switch()&0x08) // SWITCH 4 Terminal Output on/off startGateStop = true; else startGateStop = false; } bool terminalMode() { return ((TFC_GetDIP_Switch()>>3)&0x01 == 1); } 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) { 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) { 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) { 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) { 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) { 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) { 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) { 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) { 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) { 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) { 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) { 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) { 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 ReadPot1 = 1; 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 ReadPot1 = TFC_ReadPot(1)+1; // pot range 0-2 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH ); KP = KP * ReadPot1; 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) { 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) { 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) { 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) { 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 if (doRisky) MaxSpeed = LUDICROUS_SPEED * ((TFC_ReadPot(0)+1)/2.0); // faster else MaxSpeed = LIGHT_SPEED * ((TFC_ReadPot(0)+1)/2.0); // slower 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 if (CurrentLinePosError > 0) { // heading right, slow right wheel down LeftDriveRatio = MAX_POWER; RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); } else if (CurrentLinePosError < 0) { // heading left, slow left wheel down LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); // note sign change due to error being negative RightDriveRatio = 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) { // right turn-- slow right wheel down a bit LeftDriveRatio = MAX_POWER; RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); } else if (CurrentLinePosError < (-1 * ErrLimit)) { // left turn-- slow left wheel down a bit LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); // note sign change due to error being negative RightDriveRatio = MAX_POWER; } else { // when in center drive full speed 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) { TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError); TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit); TERMINAL_PRINTF("MAX SPEED = %5.2f\r\n", MaxSpeed); 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) { 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; } }