Spices.h

00001 #include "mbed.h"
00002 
00003 #ifndef _SPICES_H
00004 #define _SPICES_H
00005 
00006 // *******************************************
00007 // MASTER CONTROL PROGRAM
00008 
00009 // Loop at Servo cycle (currently 50Hz / 20mS)
00010 // Camera gets updated at its own period
00011 //   (currently 50Hz / 20mS)
00012 // 
00013 //   REQUIREMENTS:
00014 //   - Control servos in response to track line position
00015 //   - Gather as much camera data as possible between
00016 //     servo update cycles to get best read on the line
00017 //     (currently 1:1 though @ 50Hz)
00018 //   - Ignore erronous signals:
00019 //       - "mud" on track
00020 //       - low contrast light situations
00021 //       - track crossing itself (all black situation)
00022 //   - Detects 'starting line' -- ignores first detect (START)
00023 //     and stops after second detect (FINISH)
00024 //
00025 //   INPUTS:
00026 //   - linescancamera data (0-127)
00027 //   - maximum speed
00028 //   - filter settings:
00029 //       A = 15 (number of pixels at each end to ignore)
00030 //       B = 20 (estimated width of line)(possible issue if line far away)
00031 //       C = 2^12 (max value based on ADC sample rate of 12 bits)
00032 //       D = Edge threshold of Derivative (max / 4 = 2^10)
00033 //   - control parameters, KP and KD
00034 //
00035 //   CONTROL ALGORITHM:
00036 //   1 - Get Line Position
00037 //   2 - Compute Error from Line Position
00038 //   3 - Set Servo position based on Error and Derivative of Error
00039 //   4 - Store error for next cycle to perform derivative
00040 //   
00041 //
00042 //   GET LINE POSITION
00043 //     INPUTS: linescan data
00044 //     RETURNS: either position or starting gate flag
00045 //   ALGO:
00046 //   - Find line edges:
00047 //      - filter out first and last A number of pixels from left and right
00048 //      - calculate derivative of data
00049 //      - search for line edges in derivative data (search for either track line or starting gate)
00050 //        - Search for all pixels with Value < -D, indicating negative edge (large negative means going from white to black)
00051 //        - Search for all pixels with Value > D, indicating positive edge (large positive means going from black to white)
00052 //        - Clean up adjacent line edges-- edges 1 pixel away from each other combined to be considered a single edge, average pixel value
00053 //   - if starting gate then send up flag
00054 //   - if track, calculate position
00055 //
00056 //
00057 //   COMPUTE LINE ERROR
00058 //   - take line position and target-- calculate error (negative or positive)
00059 //
00060 //   SET SERVO POSITION 
00061 //      -  int servoPosition = KP * Error + KD * (Error - lastError);
00062 //      -  lastError = Error;
00063 //
00064 //
00065 //   TBD:
00066 //   - store multiple edge positions in order to average prior to next servo update
00067 //
00068 // *******************************************
00069 //
00070 void TrackMode();
00071 
00072 uint16_t getMode();
00073 bool terminalMode();
00074 
00075 // prints out line scan data for you
00076 void printLineScanData(uint16_t *LineScanData);
00077 
00078 // calculates derivative of line scan data
00079 void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut);
00080 
00081 // prints out derivative of line scan data -- assumes deriv already calculated
00082 void printDerivLineScanData(float* derivLineScanData);
00083 
00084 // serves to grab a frame of camera data
00085 void grabCameraFrame();
00086 
00087 // find negative and positive edges in image data, store in array, combines edges found in adjacent pixels
00088 void findEdges(float* derivLineScanData);
00089 
00090 // improved algo to find negative and positive edges in image data, store in array, combines edges found in adjacent pixels
00091 void findEdges_v2(float* derivLineScanData);
00092 
00093 // prints out edge data found
00094 void printEdgesFound();
00095 
00096 // review edge data
00097 //   - report line position if there
00098 //   - set starting gate flag if there
00099 //   - do nothing with position value otherwise
00100 void reviewEdges();
00101 
00102 // Decide new actions based on track status
00103 void ActOnTrackStatus();
00104 
00105 // Update Steering settings
00106 void SteeringControl();
00107 
00108 // Apply steering setting to servo!
00109 void Steer();
00110 
00111 // Update speed settings
00112 void SpeedControl();
00113 
00114 // Apply speed settings to motors!
00115 void Drive();
00116 
00117 // Adjust parameters based on max light measured value
00118 void adjustLights();
00119 
00120 // print out light adjust data
00121 void printAdjustLightsData();
00122 
00123 // Give user feedback as to detection state via LEDs
00124 void feedbackLights();
00125 
00126 void useMode();
00127      
00128 // capture log data
00129 void captureData();
00130 
00131 // dump log data to terminal
00132 void dumpData();
00133            
00134 #endif