Diff: LineScan.cpp

Revision:

33:cc3810ac5365

Parent:

28:3006d46dcec5

Child:

34:5c35287ff49e

Child:

35:ac9f45bd5352

--- a/LineScan.cpp	Thu May 21 20:27:53 2015 +0000
+++ b/LineScan.cpp	Fri Nov 13 02:23:16 2015 +0000
@@ -10,6 +10,8 @@
 #define FIND_PEAK 1
 #define FIND_TROUGH 2
 
+#define IDX_ARY_LEN 10
+
 uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
     uint16_t pixel;
     
@@ -31,40 +33,42 @@
     *camClk = 0;
 }
 
-float processFn(uint16_t *array, int arraySz, int* exposure){//, telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff){
+float processFn(uint16_t *array, int arraySz, int* exposure, bool preferLeftLine, int *numLines){
     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_ary[10];
-    int h_idx_pos = 0;
-    int l_idx_ary[10];
-    int l_idx_pos = 0;
+    int h_idx_ary[IDX_ARY_LEN];
+    int h_idx_pos = 1;
+    int l_idx_ary[IDX_ARY_LEN];
+    int l_idx_pos = 1;
     int state = NEUTRAL;
     int *l_walker, *h_walker;
-    int out_width = frameLen;
     int i;
     float out = -1.0;
     
     //for AGC
     float exposureChange;
     
+    //line counter initialization
+    *numLines = 0;
+    
     h_idx_ary[0] = -1;
     l_idx_ary[0] = -1;
+    h_idx_ary[1] = -1;
+    l_idx_ary[1] = -1;
     
     if(array){    
         avg[0] = array[SKIP - 1]/2 + array[SKIP]/2;
         diff[0] = 0;
-        //tele_linescan_diff[0] = diff[0];
         
         total += avg[0];    //AGC
         
         for(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
             
@@ -126,24 +130,40 @@
         else if(*exposure > UPDATE_RATE)
             *exposure = UPDATE_RATE;
     }
-    
-    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
-        //width needs to be larger than 2 pixels wide to be a good read.
+    if(!preferLeftLine){ //TODO verify
+        //search left to right and return first result
+        l_walker = l_idx_ary + 1;
+        h_walker = h_idx_ary + 1;
         
-        if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width && (*h_walker - *l_walker) > 2){
-            out_width = *h_walker - *l_walker;
-            out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen);    //0.5 is center
+        while(*l_walker != -1 && *h_walker != -1 && out < 0){
+            //evaluate out and advance if black line on white background and returns center of smallest black band
+            //if interval is black on white, advance the pointer for the peak array
+            //width needs to be larger than 2 pixels wide to be a good read.
+            
+            if(*h_walker > *l_walker && (*h_walker - *l_walker) < 10 && (*h_walker - *l_walker) > 2){
+                out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen);    //0.5 is center
+                *numLines++;
+                h_walker++;
+            }
             l_walker++;
         }
-        h_walker++;
+    } else {
+         //search right to left and return first result
+        l_walker = l_idx_ary + IDX_ARY_LEN - 1;
+        h_walker = h_idx_ary + IDX_ARY_LEN - 1;
         
-        //serial.printf("%d %d\r\n", *h_walker, *l_walker);
-        //serial.printf("%.2f\r\n", out);
+        while(*l_walker != -1 && *h_walker != -1 && out < 0){
+            //evaluate out and advance if black line on white background and returns center of smallest black band
+            //if interval is black on white, advance the pointer for the peak array
+            //width needs to be larger than 2 pixels wide to be a good read.
+            
+            if(*h_walker > *l_walker && (*h_walker - *l_walker) < 10 && (*h_walker - *l_walker) > 2){
+                out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen);    //0.5 is center
+                *numLines++;
+                l_walker--;
+            }
+            h_walker--;
+        }
     }
     
     return out;
@@ -154,8 +174,9 @@
                  DigitalOut *camSi, 
                  DigitalOut *camClk, 
                  int *exposure, 
-                 telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan){//,
-                 //telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan_diff){
+                 bool preferLeft,
+                 int *lineCt,
+                 telemetry::NumericArray<uint16_t, NUM_PIX> &tele_linescan){
     uint16_t lineAry[NUM_PIX];
     float position;
     
@@ -167,8 +188,7 @@
     }
     
     //process line scan data
-    position  = processFn(lineAry, NUM_PIX, exposure);//, tele_linescan_diff);
-    //serial.printf("%.2f\r\n", position);
+    position  = processFn(lineAry, NUM_PIX, exposure, preferLeft, lineCt);        //TODO integrate line side preference
     
     return position;
 }