Diff: main.cpp

Revision:

14:8b17b298b4dd

Parent:

13:e276703d5a5d

Child:

15:7f21c08be164

diff -r e276703d5a5d -r 8b17b298b4dd main.cpp
--- a/main.cpp	Thu Jul 10 14:13:15 2014 +0000
+++ b/main.cpp	Thu Jul 10 14:52:32 2014 +0000
@@ -25,9 +25,11 @@
     int black_centerR, black_centerL, center, times = 0;
     int flag = 0, sp = 64;
     int t = 0;
-    double last = 0.0,avgg = middle;
+    double last = 0.0;
+    double avgg = middle,last_avgg = middle, next_avgg = middle;
+    double v = 0.4;
     
-    char psudo_line[128];
+    //char psudo_line[128];
     
 #ifdef Debug_cam_uart
      pc.baud(115200);
@@ -160,20 +162,22 @@
             
             avg = last_turn / times;
             if(avg < 0.042 && avg > 0.034){
-                MotorA.rotate(0.3);
-                MotorB.rotate(0.3);
+                v = 0.4;
             }
             else{
-                MotorA.rotate(0.18);
-                MotorB.rotate(0.18);
+                v = 0.18;
             }
-                
-            servo.set_angle(avg);
+            MotorA.rotate(v);
+            MotorB.rotate(v);
+            servo.set_angle((avg*7 + next_avgg)/8);
+            //servo.set_angle(avg);
             times = last_turn = 0;
             
         }
-        if(t == 600){
+        if(t == 500){
+            last_avgg = avgg;
             avgg = last / t;
+            next_avgg = 2*avgg -last_avgg;
             t = 0;
             last = 0.0;
         }