Diff: PsiSwarm/sensors.cpp

Revision:

3:cd048f6e544e

Parent:

2:a6214fd156ff

Child:

6:ff3c66f7372b

--- a/PsiSwarm/sensors.cpp	Mon Oct 05 14:42:16 2015 +0000
+++ b/PsiSwarm/sensors.cpp	Mon Oct 05 20:42:37 2015 +0000
@@ -284,13 +284,66 @@
 }
     
 // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm
-void store_line_position ( void ){
+void store_line_position ( ){
     // Store background and reflected base IR values
     store_base_ir_values();
-    
+    int h_value[5];
+    int line_threshold = 1000;
+    int line_threshold_hi = 2000;
+    char count = 0;
+    line_found = 0;
+    line_position = 0;
+    for(int i=0;i<5;i++){
+        if(get_background_base_ir_value(i) > get_illuminated_base_ir_value(i)) h_value[i]=0;
+        else h_value[i] = get_illuminated_base_ir_value(i) - get_background_base_ir_value(i);
+        if(h_value[i] < line_threshold) count++;
+    }    
+    if(count == 1){
+       line_found = 1;
+       if(h_value[0] < line_threshold) {
+           line_position = -1;
+           if(h_value[1] < line_threshold_hi) line_position = -0.8;   
+       }
+       
+       if (h_value[1] < line_threshold) {
+            line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);;   
+       }
+       if(h_value[2] < line_threshold) {
+            line_position = (0.00005 * h_value[1]) - (0.0001 * h_value[3]);
+       }
+       if(h_value[3] < line_threshold) {
+            line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);;   
+       }
+       if(h_value[4] < line_threshold) {
+           line_position = 1; 
+           if(h_value[3] < line_threshold_hi) line_position = 0.8;  
+       }
+    }
+    if(count == 2){
+        if(h_value[0] && h_value[1] < line_threshold){
+            line_found = 1;
+            line_position = -0.6;
+        }
+        
+        if(h_value[1] && h_value[2] < line_threshold){
+            line_found = 1;
+            line_position = -0.4;
+        }
+        
+        if(h_value[2] && h_value[3] < line_threshold){
+            line_found = 1;
+            line_position = 0.4;
+        }
+        
+        if(h_value[3] && h_value[4] < line_threshold){
+            line_found = 1;
+            line_position = 0.6;
+        }
+    }
 }
 
 
+
 void calibrate_base_ir_sensors (void) {
     short white_background[5];
     short white_active[5];