Diff: main.cpp

Revision:

9:a891053657f3

Parent:

8:c2c3aee85c2d

Child:

10:e11f82b40d53

--- a/main.cpp	Tue Jul 08 04:36:09 2014 +0000
+++ b/main.cpp	Wed Jul 09 14:08:11 2014 +0000
@@ -6,9 +6,7 @@
 
 #define Debug_cam_uart
 
-//left   0.015
-//middle 0.045
-//right  0.072
+
 Serial pc(USBTX, USBRX);
 BX_servo servo; 
 BX_camera cam;
@@ -19,13 +17,11 @@
 
 
 int main() {
-  /*  
-    int black_va;
-    int white_va;
-    */
-    double left = 0.016, middle = 0.045, right = 0.071;
-    double error, turn, last_turn = middle;
-    double Kp = 0.000527;
+  
+    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;
     
     char psudo_line[128];
@@ -37,6 +33,7 @@
   while(1){   
      
         cam.read();
+
         MotorA.rotate(0.0);
         MotorB.rotate(0.0);
         
@@ -47,18 +44,31 @@
         MotorA.rotate(0.2);
         MotorB.rotate(0.2);
         
+
+        if(times == 10){
+            avg = last_turn / 10;
+            last_turn = times = 0;
+        }
+
         if(black_centerL == 128 && black_centerR == 0){//no line
             
-            turn = middle;
+            turn = avg;
             
         } else if (black_centerL == 128 && black_centerR != 0){//no left line
-        
-            turn = (middle + left) / 2;
+            
+
+            /*error = 64 - black_centerR;
+            turn = Kp * error + middle;*/
+            turn = avg;
+
             
         
         } else if (black_centerL != 128 && black_centerR == 0){//no right line
             
-            turn = (middle + right) / 2;
+
+            /*error = 64 - black_centerL;
+            turn = Kp * error + middle;*/
+            turn = avg;
             
         } else {
             if(60 < center && center < 68){
@@ -70,24 +80,22 @@
                 turn = Kp * error + middle;
             }
         }
-/*
-        if(turn == middle){
-            if (last_turn < 0.025){
-                
-                servo.set_angle(last_turn);
-                wait_ms(200);
-                
-            } else if (last_turn > 0.066){
-                
-                servo.set_angle(last_turn);
-                wait_ms(200);
+
+        /*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;
             }
-            servo.set_angle(turn);
-        } else {
-            servo.set_angle(turn);
-        }
-        last_turn = turn;*/
-        
+
+            avg = times = 0;
+        }*/
+
+        last_turn += turn;
+        times++;
         servo.set_angle(turn);
         
         
@@ -102,7 +110,7 @@
             pc.printf("%c", psudo_line[i]);    
         }
         pc.printf("\r\n");
-        pc.printf("black centerL: %d   black_centerR: %d   psudo_line: %d turn: %lf\r\n", black_centerL, black_centerR, center, turn);
+        pc.printf("black centerL: %d   black_centerR: %d   psudo_line: %d turn: %lf avg: %lf\r\n", black_centerL, black_centerR, center, turn, avg);
         
         
         /*for(int i = 128; i > 64;i--){