Diff: main.cpp

Revision:

4:ff05e1fe33ce

Parent:

3:9902261b1156

Child:

8:1af0c46d21d2

--- a/main.cpp	Sat Mar 11 13:08:06 2017 +0000
+++ b/main.cpp	Sat Mar 11 13:56:22 2017 +0000
@@ -90,11 +90,28 @@
             
         //if the second value is smaller than the first value
         //steer right
+        int value_to_steer_UCL = 1; 
+        int value_to_steer_LCL = -1; 
+        int val1=0;
+        int val2=0;
+        int val3=0;
+        int val4=0;
+        int val5=0;
+        val1 = mean_measured1[1] - mean_measured1[0];
+        val2 = mean_measured2[1] - mean_measured2[0];
+        val3 = mean_measured3[1] - mean_measured3[0];
+        val4 = mean_measured4[1] - mean_measured4[0];
+        val5 = mean_measured5[1] - mean_measured5[0];
         
-        //if the second value is bigger than first value
-        //steer left
-        
-        // with respect to a deadband (error)
+        if((val2 < value_to_steer_LCL) && (val4 > value_to_steer_UCL)){
+            //turn right
+            }
+        else if((val2 > value_to_steer_UCL) && (val4 < value_to_steer_LCL)){
+            //turn left
+            }
+        else {
+            // drive
+            }
         
         //drive at 1 speed
         iMean +=1;