Diff: main.cpp

Revision:

10:e11f82b40d53

Parent:

9:a891053657f3

Child:

11:9b4788de75fe

--- a/main.cpp	Wed Jul 09 14:08:11 2014 +0000
+++ b/main.cpp	Wed Jul 09 15:10:49 2014 +0000
@@ -20,9 +20,8 @@
   
     double left = 0.025, middle = 0.038, right = 0.054;
     double error, turn, last_turn = middle, avg = 0;
-    int times = 0;
     double Kp = 0.000527, Ki = 0.025;
-    int black_centerR, black_centerL, center;
+    int black_centerR, black_centerL, center, times = 0;
     
     char psudo_line[128];
     
@@ -45,9 +44,24 @@
         MotorB.rotate(0.2);
         
 
-        if(times == 10){
+        if(times % 10 == 0){
             avg = last_turn / 10;
-            last_turn = times = 0;
+            last_turn = 0;
+
+
+
+            if(times == 30){
+                times = 0;
+                if(avg < middle-0.003){
+
+                    avg -= 0.002;
+
+                }
+                else if(avg > middle + 0.003){
+
+                    avg += 0.002;
+                }
+            }
         }
 
         if(black_centerL == 128 && black_centerR == 0){//no line
@@ -71,6 +85,7 @@
             turn = avg;
             
         } else {
+
             if(60 < center && center < 68){
                 
                 turn = middle;
@@ -81,19 +96,6 @@
             }
         }
 
-        /*if(times == 10){
-            pc.printf(">>>>>>>>>>>>>>>\r\n\r\n\r\n\r\n\r\n\r\n\r\n");
-            avg /= 10;
-
-            if(avg > 0.045){
-                turn += Ki * avg;
-            } else if(avg < 0.037){
-                turn -= Ki * avg;
-            }
-
-            avg = times = 0;
-        }*/
-
         last_turn += turn;
         times++;
         servo.set_angle(turn);