Shumelev dima / TFC-RACING-DEMO

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

Dependencies:   FRDM-TFC

Fork of TFC-RACING-DEMO by Daniel Hadad

Spices/Spices.h@1:2ca599933725, 2014-04-28 (annotated)

Committer:
morgonXak
Date:
Mon Apr 28 15:05:37 2014 +0000
Revision:
1:2ca599933725
Parent:
0:98e98e01a6ce 
jguh; \; ;

Who changed what in which revision?

UserRevisionLine numberNew 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