Diff: main.cpp

Revision:

22:f097de115024

Parent:

21:df5a530208eb

--- a/main.cpp	Mon Mar 02 17:59:55 2015 +0000
+++ b/main.cpp	Mon Mar 02 19:57:48 2015 +0000
@@ -32,6 +32,9 @@
     int black_value_count = 0;
     int black_center_value = 0;
     int sum_black = 0;
+    int black_center_value_past_1 =64;
+    int black_center_value_current = 64;
+    
     int violence_level = 0;
     
     int center_now = 64;
@@ -41,6 +44,7 @@
     int center_past_4 = 64;
     //int best_guess_center = 64;
     
+    
     int set_point = 64;
     int previous_error = 0;
     int error = 0;
@@ -142,7 +146,7 @@
 
                     // checking for center line (single line)
                     for (uint16_t i=0; i<128; i++) {
-                        if ((*(TFC_LineScanImage0+i) < 260)) {
+                        if ((*(TFC_LineScanImage0+i) < 300)) {
                             black_values_list[black_value_count] = i;
                             black_value_count++;
                         }
@@ -163,7 +167,9 @@
                     center_now = sum_black / black_value_count;
                     
                     // best guess of center based on weighted average of history
-                    black_center_value = (5*center_now + 5*center_past_1 + 10*center_past_2 +10*center_past_3 +70*center_past_4)/100;
+                    black_center_value_past_1 = black_center_value_current ;
+                    black_center_value_current = (5*center_now + 5*center_past_1 + 20*center_past_2 +30*center_past_3 +40*center_past_4)/100;
+                    black_center_value = (60*black_center_value_current + 40*black_center_value_past_1)/100;
                     
                     /* ******* PID ALGORITHM *******
                     
@@ -191,7 +197,9 @@
                     // need to turn left
                     if (black_center_value < 64) {
                         
-                        current_servo_position= float(.01875*black_center_value-(1.2));
+                        //current_servo_position= float(.01875*black_center_value-(1.2));
+                        //current_servo_position= float(.015625*black_center_value-(1.0));
+                        current_servo_position= float(.0234375*black_center_value-(1.5));
                         if(current_servo_position <= -0.4)
                             current_servo_position = -0.4;
                         TFC_SetServo(0, current_servo_position);
@@ -231,7 +239,9 @@
                     // need to turn right
                     if (black_center_value > 64) {
 
-                        current_servo_position= float(.01875*black_center_value-(1.2));
+                        //current_servo_position= float(.01875*black_center_value-(1.2));
+                        //current_servo_position= float(.015625*black_center_value-(1.0));
+                        current_servo_position= float(.0234375*black_center_value-(1.5));
                         if( current_servo_position >= +0.4)
                             current_servo_position = +0.4;
                         TFC_SetServo(0, current_servo_position);