Diff: LineScan.cpp

Revision:

38:1ac5f0ab5ae6

Parent:

37:b76faff78354

Child:

39:ea0660f7d724

--- a/LineScan.cpp	Thu Nov 19 23:00:20 2015 +0000
+++ b/LineScan.cpp	Sat Nov 21 04:19:29 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,28 +33,33 @@
     *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 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;
@@ -127,25 +134,48 @@
             *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 4 pixels wide to be a good read.
+    if(preferLeftLine){
+        l_walker = l_idx_ary + IDX_ARY_LEN - 1;
+        h_walker = h_idx_ary + IDX_ARY_LEN - 1;
         
-        if(*h_walker > *l_walker && (*h_walker - *l_walker) < out_width && (*h_walker - *l_walker) > 3){
-            out_width = *h_walker - *l_walker;
-            out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen);    //0.5 is center
-            return out;
+        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 4 pixels wide to be a good read.
+            
+            if(*h_walker > *l_walker /*&& (*h_walker - *l_walker) < out_width*/ && (*h_walker - *l_walker) > 3){
+                //out_width = *h_walker - *l_walker;
+                out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen);    //0.5 is center
+                
+                (*numLines)++;
+                h_walker--;
+            }
+            l_walker--;
             
-            l_walker++;
+            //serial.printf("%d %d\r\n", *h_walker, *l_walker);
+            //serial.printf("%.2f\r\n", out);
         }
-        h_walker++;
+    } else {
+        l_walker = l_idx_ary + 1;
+        h_walker = h_idx_ary + 1;
         
-        //serial.printf("%d %d\r\n", *h_walker, *l_walker);
-        //serial.printf("%.2f\r\n", out);
+        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 4 pixels wide to be a good read.
+            
+            if(*h_walker > *l_walker /*&& (*h_walker - *l_walker) < out_width*/ && (*h_walker - *l_walker) > 3){
+                //out_width = *h_walker - *l_walker;
+                out = ((float)(*h_walker + *l_walker)) / (2.0 * (float)frameLen);    //0.5 is center
+                
+                (*numLines)++;
+                l_walker++;
+            }
+            h_walker++;
+            
+            //serial.printf("%d %d\r\n", *h_walker, *l_walker);
+            //serial.printf("%.2f\r\n", out);
+        }
     }
     
     return out;
@@ -156,8 +186,10 @@
                  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;
     
@@ -169,7 +201,7 @@
     }
     
     //process line scan data
-    position  = processFn(lineAry, NUM_PIX, exposure);//, tele_linescan_diff);
+    position  = processFn(lineAry, NUM_PIX, exposure, preferLeft, lineCt);//, tele_linescan_diff);
     //serial.printf("%.2f\r\n", position);
     
     return position;