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 MasterControlProgram();
00071 
00072 
00073 // prints out line scan data for you
00074 void printLineScanData(uint16_t *LineScanData);
00075 
00076 // calculates derivative of line scan data
00077 void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut);
00078 
00079 // prints out derivative of line scan data -- assumes deriv already calculated
00080 void printDerivLineScanData(float* derivLineScanData);
00081 
00082 // serves to grab a frame of camera data
00083 void grabCameraFrame();
00084 
00085 // find negative and positive edges in image data, store in array, combines edges found in adjacent pixels
00086 void findEdges(float* derivLineScanData);
00087 
00088 // improved algo to find negative and positive edges in image data, store in array, combines edges found in adjacent pixels
00089 void findEdges_v2(float* derivLineScanData);
00090 
00091 // prints out edge data found
00092 void printEdgesFound();
00093 
00094 // review edge data
00095 //   - report line position if there
00096 //   - set starting gate flag if there
00097 //   - do nothing with position value otherwise
00098 void reviewEdges();
00099 
00100 // Decide new actions based on track status
00101 void ActOnTrackStatus();
00102 
00103 // Update Steering settings
00104 void SteeringControl();
00105 
00106 // Apply steering setting to servo!
00107 void Steer();
00108 
00109 // Update speed settings
00110 void SpeedControl();
00111 
00112 // Apply speed settings to motors!
00113 void Drive();
00114 
00115 // Adjust parameters based on max light measured value
00116 void adjustLights();
00117 
00118 // print out light adjust data
00119 void printAdjustLightsData();
00120 
00121 // Give user feedback as to detection state via LEDs
00122 void feedbackLights();
00123 
00124 // read DIP switches and potentiometers for setting changes
00125 void readSwitches();
00126      
00127 // capture log data
00128 void captureData();
00129 
00130 // dump log data to terminal
00131 void dumpData();
00132            
00133 #endif