RACE CAR
Fork of TFC-RACING-DEMO by
Spices/Spices.h@0:98e98e01a6ce, 2014-04-20 (annotated)
- Committer:
- redxeth
- Date:
- Sun Apr 20 03:33:03 2014 +0000
- Revision:
- 0:98e98e01a6ce
Initial version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
redxeth | 0:98e98e01a6ce | 1 | #include "mbed.h" |
redxeth | 0:98e98e01a6ce | 2 | |
redxeth | 0:98e98e01a6ce | 3 | #ifndef _SPICES_H |
redxeth | 0:98e98e01a6ce | 4 | #define _SPICES_H |
redxeth | 0:98e98e01a6ce | 5 | |
redxeth | 0:98e98e01a6ce | 6 | // ******************************************* |
redxeth | 0:98e98e01a6ce | 7 | // MASTER CONTROL PROGRAM |
redxeth | 0:98e98e01a6ce | 8 | |
redxeth | 0:98e98e01a6ce | 9 | // Loop at Servo cycle (currently 50Hz / 20mS) |
redxeth | 0:98e98e01a6ce | 10 | // Camera gets updated at its own period |
redxeth | 0:98e98e01a6ce | 11 | // (currently 50Hz / 20mS) |
redxeth | 0:98e98e01a6ce | 12 | // |
redxeth | 0:98e98e01a6ce | 13 | // REQUIREMENTS: |
redxeth | 0:98e98e01a6ce | 14 | // - Control servos in response to track line position |
redxeth | 0:98e98e01a6ce | 15 | // - Gather as much camera data as possible between |
redxeth | 0:98e98e01a6ce | 16 | // servo update cycles to get best read on the line |
redxeth | 0:98e98e01a6ce | 17 | // (currently 1:1 though @ 50Hz) |
redxeth | 0:98e98e01a6ce | 18 | // - Ignore erronous signals: |
redxeth | 0:98e98e01a6ce | 19 | // - "mud" on track |
redxeth | 0:98e98e01a6ce | 20 | // - low contrast light situations |
redxeth | 0:98e98e01a6ce | 21 | // - track crossing itself (all black situation) |
redxeth | 0:98e98e01a6ce | 22 | // - Detects 'starting line' -- ignores first detect (START) |
redxeth | 0:98e98e01a6ce | 23 | // and stops after second detect (FINISH) |
redxeth | 0:98e98e01a6ce | 24 | // |
redxeth | 0:98e98e01a6ce | 25 | // INPUTS: |
redxeth | 0:98e98e01a6ce | 26 | // - linescancamera data (0-127) |
redxeth | 0:98e98e01a6ce | 27 | // - maximum speed |
redxeth | 0:98e98e01a6ce | 28 | // - filter settings: |
redxeth | 0:98e98e01a6ce | 29 | // A = 15 (number of pixels at each end to ignore) |
redxeth | 0:98e98e01a6ce | 30 | // B = 20 (estimated width of line)(possible issue if line far away) |
redxeth | 0:98e98e01a6ce | 31 | // C = 2^12 (max value based on ADC sample rate of 12 bits) |
redxeth | 0:98e98e01a6ce | 32 | // D = Edge threshold of Derivative (max / 4 = 2^10) |
redxeth | 0:98e98e01a6ce | 33 | // - control parameters, KP and KD |
redxeth | 0:98e98e01a6ce | 34 | // |
redxeth | 0:98e98e01a6ce | 35 | // CONTROL ALGORITHM: |
redxeth | 0:98e98e01a6ce | 36 | // 1 - Get Line Position |
redxeth | 0:98e98e01a6ce | 37 | // 2 - Compute Error from Line Position |
redxeth | 0:98e98e01a6ce | 38 | // 3 - Set Servo position based on Error and Derivative of Error |
redxeth | 0:98e98e01a6ce | 39 | // 4 - Store error for next cycle to perform derivative |
redxeth | 0:98e98e01a6ce | 40 | // |
redxeth | 0:98e98e01a6ce | 41 | // |
redxeth | 0:98e98e01a6ce | 42 | // GET LINE POSITION |
redxeth | 0:98e98e01a6ce | 43 | // INPUTS: linescan data |
redxeth | 0:98e98e01a6ce | 44 | // RETURNS: either position or starting gate flag |
redxeth | 0:98e98e01a6ce | 45 | // ALGO: |
redxeth | 0:98e98e01a6ce | 46 | // - Find line edges: |
redxeth | 0:98e98e01a6ce | 47 | // - filter out first and last A number of pixels from left and right |
redxeth | 0:98e98e01a6ce | 48 | // - calculate derivative of data |
redxeth | 0:98e98e01a6ce | 49 | // - search for line edges in derivative data (search for either track line or starting gate) |
redxeth | 0:98e98e01a6ce | 50 | // - Search for all pixels with Value < -D, indicating negative edge (large negative means going from white to black) |
redxeth | 0:98e98e01a6ce | 51 | // - Search for all pixels with Value > D, indicating positive edge (large positive means going from black to white) |
redxeth | 0:98e98e01a6ce | 52 | // - Clean up adjacent line edges-- edges 1 pixel away from each other combined to be considered a single edge, average pixel value |
redxeth | 0:98e98e01a6ce | 53 | // - if starting gate then send up flag |
redxeth | 0:98e98e01a6ce | 54 | // - if track, calculate position |
redxeth | 0:98e98e01a6ce | 55 | // |
redxeth | 0:98e98e01a6ce | 56 | // |
redxeth | 0:98e98e01a6ce | 57 | // COMPUTE LINE ERROR |
redxeth | 0:98e98e01a6ce | 58 | // - take line position and target-- calculate error (negative or positive) |
redxeth | 0:98e98e01a6ce | 59 | // |
redxeth | 0:98e98e01a6ce | 60 | // SET SERVO POSITION |
redxeth | 0:98e98e01a6ce | 61 | // - int servoPosition = KP * Error + KD * (Error - lastError); |
redxeth | 0:98e98e01a6ce | 62 | // - lastError = Error; |
redxeth | 0:98e98e01a6ce | 63 | // |
redxeth | 0:98e98e01a6ce | 64 | // |
redxeth | 0:98e98e01a6ce | 65 | // TBD: |
redxeth | 0:98e98e01a6ce | 66 | // - store multiple edge positions in order to average prior to next servo update |
redxeth | 0:98e98e01a6ce | 67 | // |
redxeth | 0:98e98e01a6ce | 68 | // ******************************************* |
redxeth | 0:98e98e01a6ce | 69 | // |
redxeth | 0:98e98e01a6ce | 70 | void MasterControlProgram(); |
redxeth | 0:98e98e01a6ce | 71 | |
redxeth | 0:98e98e01a6ce | 72 | |
redxeth | 0:98e98e01a6ce | 73 | // prints out line scan data for you |
redxeth | 0:98e98e01a6ce | 74 | void printLineScanData(uint16_t *LineScanData); |
redxeth | 0:98e98e01a6ce | 75 | |
redxeth | 0:98e98e01a6ce | 76 | // calculates derivative of line scan data |
redxeth | 0:98e98e01a6ce | 77 | void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut); |
redxeth | 0:98e98e01a6ce | 78 | |
redxeth | 0:98e98e01a6ce | 79 | // prints out derivative of line scan data -- assumes deriv already calculated |
redxeth | 0:98e98e01a6ce | 80 | void printDerivLineScanData(float* derivLineScanData); |
redxeth | 0:98e98e01a6ce | 81 | |
redxeth | 0:98e98e01a6ce | 82 | // serves to grab a frame of camera data |
redxeth | 0:98e98e01a6ce | 83 | void grabCameraFrame(); |
redxeth | 0:98e98e01a6ce | 84 | |
redxeth | 0:98e98e01a6ce | 85 | // find negative and positive edges in image data, store in array, combines edges found in adjacent pixels |
redxeth | 0:98e98e01a6ce | 86 | void findEdges(float* derivLineScanData); |
redxeth | 0:98e98e01a6ce | 87 | |
redxeth | 0:98e98e01a6ce | 88 | // improved algo to find negative and positive edges in image data, store in array, combines edges found in adjacent pixels |
redxeth | 0:98e98e01a6ce | 89 | void findEdges_v2(float* derivLineScanData); |
redxeth | 0:98e98e01a6ce | 90 | |
redxeth | 0:98e98e01a6ce | 91 | // prints out edge data found |
redxeth | 0:98e98e01a6ce | 92 | void printEdgesFound(); |
redxeth | 0:98e98e01a6ce | 93 | |
redxeth | 0:98e98e01a6ce | 94 | // review edge data |
redxeth | 0:98e98e01a6ce | 95 | // - report line position if there |
redxeth | 0:98e98e01a6ce | 96 | // - set starting gate flag if there |
redxeth | 0:98e98e01a6ce | 97 | // - do nothing with position value otherwise |
redxeth | 0:98e98e01a6ce | 98 | void reviewEdges(); |
redxeth | 0:98e98e01a6ce | 99 | |
redxeth | 0:98e98e01a6ce | 100 | // Decide new actions based on track status |
redxeth | 0:98e98e01a6ce | 101 | void ActOnTrackStatus(); |
redxeth | 0:98e98e01a6ce | 102 | |
redxeth | 0:98e98e01a6ce | 103 | // Update Steering settings |
redxeth | 0:98e98e01a6ce | 104 | void SteeringControl(); |
redxeth | 0:98e98e01a6ce | 105 | |
redxeth | 0:98e98e01a6ce | 106 | // Apply steering setting to servo! |
redxeth | 0:98e98e01a6ce | 107 | void Steer(); |
redxeth | 0:98e98e01a6ce | 108 | |
redxeth | 0:98e98e01a6ce | 109 | // Update speed settings |
redxeth | 0:98e98e01a6ce | 110 | void SpeedControl(); |
redxeth | 0:98e98e01a6ce | 111 | |
redxeth | 0:98e98e01a6ce | 112 | // Apply speed settings to motors! |
redxeth | 0:98e98e01a6ce | 113 | void Drive(); |
redxeth | 0:98e98e01a6ce | 114 | |
redxeth | 0:98e98e01a6ce | 115 | // Adjust parameters based on max light measured value |
redxeth | 0:98e98e01a6ce | 116 | void adjustLights(); |
redxeth | 0:98e98e01a6ce | 117 | |
redxeth | 0:98e98e01a6ce | 118 | // print out light adjust data |
redxeth | 0:98e98e01a6ce | 119 | void printAdjustLightsData(); |
redxeth | 0:98e98e01a6ce | 120 | |
redxeth | 0:98e98e01a6ce | 121 | // Give user feedback as to detection state via LEDs |
redxeth | 0:98e98e01a6ce | 122 | void feedbackLights(); |
redxeth | 0:98e98e01a6ce | 123 | |
redxeth | 0:98e98e01a6ce | 124 | // read DIP switches and potentiometers for setting changes |
redxeth | 0:98e98e01a6ce | 125 | void readSwitches(); |
redxeth | 0:98e98e01a6ce | 126 | |
redxeth | 0:98e98e01a6ce | 127 | // capture log data |
redxeth | 0:98e98e01a6ce | 128 | void captureData(); |
redxeth | 0:98e98e01a6ce | 129 | |
redxeth | 0:98e98e01a6ce | 130 | // dump log data to terminal |
redxeth | 0:98e98e01a6ce | 131 | void dumpData(); |
redxeth | 0:98e98e01a6ce | 132 | |
redxeth | 0:98e98e01a6ce | 133 | #endif |