Diff: newAlgorithm.cpp

Revision:

42:c189b914931d

Parent:

41:79ccb6f70ab6

Child:

43:9f541718be01

--- a/newAlgorithm.cpp	Tue Mar 10 13:51:08 2015 +0000
+++ b/newAlgorithm.cpp	Tue Mar 10 15:51:02 2015 +0000
@@ -22,6 +22,9 @@
 const float CONSERVATIVE =.30;
 const float STOP =0;
 
+const bool linescan_enable = true;
+const bool image = true;
+
 const int black_values_list[LINE_SCAN_LENGTH];
 const int black_value_count = 0;
 const int black_center_value = 0;
@@ -109,12 +112,15 @@
         {
             if (TFC_LineScanImageReady !=0) 
             {
-                if (linescan_ping_pong) 
-                {
-                    position = getSensorValues(); // get position
-                    error = pid_calc(position); // send position to PID algorithm to get error
-                    calcMotors(error); // send error to calculate both motors
-                }
+                // first image
+                position = getSensorValues(image); // get position
+                error = pid_calc(position); // send position to PID algorithm to get error
+                calcMotors(error); // send error to calculate both motors
+                
+                // second image
+                position = getSensorValues(image);
+                error = pid_calc(position);
+                calcMotors(error);
             }
         }
         else
@@ -123,15 +129,32 @@
         }
     }
     
-    int getSensorValues()
+    int getSensorValues(bool image)
     {        
-        for (uint16_t i=0; i<128; i++) 
+        if(image)
         {
-            if ((*(TFC_LineScanImage0+i) < 250)) 
+            for (uint16_t i=0; i<128; i++) 
             {
-                black_values_list[black_value_count] = i;
-                black_value_count++;
+                if ((*(TFC_LineScanImage1+i) < 250)) 
+                {
+                    black_values_list[black_value_count] = i;
+                    black_value_count++;
+                }
             }
+            
+            image = false;
+        }
+        else
+        {
+            for (uint16_t i=0; i<128; i++)
+            {
+                if ((*(TFC_LineScanImage0+i) < 250))
+                {
+                    black_values_list[black_value_count] = i;
+                    black_value_count++;
+                }
+            }
+            image = true;
         }
 
         for(int i=0; i<black_value_count; i++) 
@@ -139,9 +162,9 @@
             sum_black += black_values_list[i];
         }
                     
-            black_center_value = int(sum_black / black_value_count);
-            return black_center_value;
-        }
+        black_center_value = int(sum_black / black_value_count);
+        return black_center_value;
+        
     }
 
     int pid_calc(int pos)