Diff: LineScan.cpp

Revision:

14:928254a609cb

Parent:

13:c17cf029d899

Child:

15:4bd1c1d2cf94

--- a/LineScan.cpp	Fri Apr 10 00:38:31 2015 +0000
+++ b/LineScan.cpp	Sat Apr 18 00:19:59 2015 +0000
@@ -5,10 +5,16 @@
 */
 #include "LineScan.h"
 
-#define THRESH 5000
+#define THRESH 5000 //may need adjusting
 
 #define MEAN_REF 20000      //ideal mean we should see
-#define CAM_CTRL_GAIN 0.0005 //should be small
+#define CAM_CTRL_GAIN 0.0009 //should be small
+
+#define NEUTRAL 0
+#define FIND_PEAK 1
+#define FIND_TROUGH 2
+
+#define FRAMELEN NUM_PIX -  2 * SKIP
 
 uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
     uint16_t pixel;
@@ -18,7 +24,7 @@
     
     //clock pulse for next pixel n + 1
     *camClk = 1;
-    *camClk = 0;    // Apparently there is no need for any delay. It may be desireable to flatten this, or perhaps the compiler does it. 
+    *camClk = 0;
     
     return pixel;  //return result as an uint16_t (16 bit integer)
 }
@@ -31,59 +37,112 @@
     *camClk = 0;
 }
 
-float processFn(uint16_t *array, int arraySz, int* exposure){
-    const static int frameLen = NUM_PIX -  2 * SKIP;
-    int avg[frameLen];
-    int diff[frameLen];
+float processFn(uint16_t *array, int arraySz, int* exposure, telemetry::NumericArray<uint16_t, NUM_PIX -  2 * SKIP> &tele_linescan_diff){   //consider adding diff to telemetry
+    //const static int FRAMELEN = NUM_PIX -  2 * SKIP;
+    int avg[FRAMELEN];
+    int diff[FRAMELEN];
     int highest = 0;
     int lowest = 0;
     int total = 0;
-    int h_idx = frameLen/2;
-    int l_idx = frameLen/2;
+    int h_idx_ary[FRAMELEN];
+    int h_idx_pos = 0;
+    int l_idx_ary[FRAMELEN];
+    int l_idx_pos = 0;
+    int state = NEUTRAL;
+    int *l_walker, *h_walker;
+    int out_width = FRAMELEN;
+    float out = -1.0;
     
     //for AGC
     float exposureChange;
     
-    //Just finds line center, does not track crossings (needs this feature)
     if(array){ 
+        memset (h_idx_ary, -1, FRAMELEN);
+        memset (l_idx_ary, -1, FRAMELEN);
+    
         avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
         diff[0] = 0;
+        tele_linescan_diff[0] = 0;
         
         total += avg[0];    //AGC
         
-        for(int i = 1; i < frameLen; i++){
+        for(int i = 1; i < FRAMELEN; i++){
             avg[i] = array[i - 1 + SKIP]/2 + array[i + SKIP]/2;   //smoothing
             diff[i] = avg[i - 1] - avg[i];          //differential
+            tele_linescan_diff[i] = diff[i];
             
             total += avg[i];    //AGC
             
-            if(diff[i] > highest){
-                highest = diff[i];
-                h_idx = i;
-            }
-            else if(diff[i] < lowest){
-                lowest = diff[i];
-                l_idx = i;
+            //Finite State Machine
+            switch(state){
+                case NEUTRAL:
+                    if( diff[i] > THRESH ){
+                        state = FIND_PEAK;
+                        highest = diff[i];
+                    }else if( diff[i] < -THRESH ){
+                        state = FIND_TROUGH;
+                        lowest = diff[i];
+                    }
+                    break;
+                    
+                case FIND_PEAK:
+                    if( diff[i] <= THRESH )
+                        state = NEUTRAL;
+                    else if(diff[i] > highest){
+                        highest = diff[i];
+                        h_idx_ary[h_idx_pos] = i;  //insert to array
+                        h_idx_pos++;
+                    }
+                    break;
+                    
+                case FIND_TROUGH:
+                    if( diff[i] >= -THRESH )
+                        state = NEUTRAL;
+                    else if(diff[i] < lowest){
+                        lowest = diff[i];
+                        l_idx_ary[l_idx_pos] = i;  //insert to array
+                        l_idx_pos++;
+                    }
+                    break;
+                    
+                default:
+                    //exit case if exception happened
+                    return out;
             }
         }
         //AGC, simple proportional controller
-        total = total / frameLen;
+        total = total / FRAMELEN;
         exposureChange = ((float)(MEAN_REF - total)) * CAM_CTRL_GAIN;    
         *exposure += (int)exposureChange;
-        if(*exposure < 1)
-            *exposure = 1;
-        else if(*exposure > 30)
-            *exposure = 30;
+        if(*exposure < 0)
+            *exposure = 0;
+        else if(*exposure > UPDATE_RATE)
+            *exposure = UPDATE_RATE;
     }
-    //valid if white line on black and not blinded
-    if(h_idx > l_idx && (highest - lowest) > THRESH)
-        return ((float)(h_idx + l_idx)) / (2.0 * (float)frameLen);    //0.5 is center
-    else
-        return -1.0;  //invalid read
+    
+    l_walker = l_idx_ary;
+    h_walker = h_idx_ary;
+    while(*l_walker != -1 && *h_walker != -1){
+        //evaluate out and advance if line is white on black and returns center of smallest white band
+        //if interval is black on white, advance the pointer for the peak array
+        if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width){
+            out_width = *h_walker - *l_walker;
+            out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)FRAMELEN);    //0.5 is center
+            l_walker++;
+        }
+        h_walker++;
+    }
+    
+    return out;
 }
 
 //call after integration time is done, returns index of array line is expected to be at and new exposure time
-float getLinePos(AnalogIn cam, DigitalOut *camSi, DigitalOut *camClk, int *exposure, telemetry::NumericArray<uint16_t, 128> &tele_linescan){
+float getLinePos(AnalogIn cam, 
+                 DigitalOut *camSi, 
+                 DigitalOut *camClk, 
+                 int *exposure, 
+                 telemetry::NumericArray<uint16_t, 128> &tele_linescan,
+                 telemetry::NumericArray<uint16_t, NUM_PIX -  2 * SKIP> &tele_linescan_diff){
     uint16_t lineAry[NUM_PIX];
     float position;
     
@@ -95,7 +154,7 @@
     }
     
     //process line scan data
-    position  = processFn(lineAry, NUM_PIX, exposure);
+    position  = processFn(lineAry, NUM_PIX, exposure, tele_linescan_diff);
     
     return position;
 }