Diff: main.cpp

Revision:

20:c728b8ffad97

Parent:

19:85eb7991e2ab

Child:

21:df5a530208eb

Child:

23:fe15b6147c1b

--- a/main.cpp	Thu Feb 26 16:30:55 2015 +0000
+++ b/main.cpp	Fri Feb 27 17:10:15 2015 +0000
@@ -34,6 +34,13 @@
     int sum_black = 0;
     int violence_level = 0;
     
+    int center_now = 64;
+    int center_past_1 = 64;
+    int center_past_2 = 64;
+    int center_past_3 = 64;
+    int center_past_4 = 64;
+    //int best_guess_center = 64;
+    
     int set_point = 64;
     int previous_error = 0;
     int error = 0;
@@ -141,8 +148,18 @@
                         sum_black += black_values_list[i];
                     }
 
+                    //update history
+                    center_past_4= center_past_3;
+                    center_past_3= center_past_2;
+                    center_past_2= center_past_1;
+                    center_past_1= center_now;
+
                     // value of center of black (single line)
-                    black_center_value= sum_black / black_value_count;
+                    //black_center_value = sum_black / black_value_count;
+                    center_now = sum_black / black_value_count;
+                    
+                    // best guess of center based on weighted average of history
+                    black_center_value = (5*center_now + 10*center_past_1 + 15*center_past_2 +30*center_past_3 +40*center_past_4)/100;
                     
                     /* ******* PID ALGORITHM *******