Diff: main.cpp

Revision:

12:a3cee7619b77

Parent:

11:9b4788de75fe

diff -r 9b4788de75fe -r a3cee7619b77 main.cpp
--- a/main.cpp	Thu Jul 10 06:58:37 2014 +0000
+++ b/main.cpp	Thu Jul 10 10:22:14 2014 +0000
@@ -17,11 +17,12 @@
 
 int main() {
   
-    double left = 0.025, middle = 0.038, right = 0.054;
+    double left = 0.025, middle = 0.038, right = 0.057;
     double error, turn, last_turn = middle, avg = middle;
-    double Kp = 0.000327, Ki = 0.025;
+    double last_error = 0;
+    double Kp = 0.000327, Ki = 0.0001;
     int black_centerR, black_centerL, center, times = 0;
-    int flag_R = 0, flag_L = 0;
+    int flag = 0, sp = 64;
     
     char psudo_line[128];
     
@@ -30,7 +31,7 @@
      
      
   while(1){   
-     
+        
         cam.read();
 
         MotorA.rotate(0.0);
@@ -41,11 +42,18 @@
         
         //center = (black_centerL + black_centerR) / 2;
         
+        /*while(1){
+            for(double i = 0.025; i < 0.058; i+= 0.001){
+                servo.set_angle(0.038);
+                wait_ms(1000);
+            
+            }
+        }*/
+
         MotorA.rotate(0.15);
         MotorB.rotate(0.15);
         
-        center = 64;
-
+        //catch little error
         if(black_centerL > 64 && black_centerR > 64){
 
             center = (black_centerL <= black_centerR) ? black_centerL : black_centerR;
@@ -70,25 +78,46 @@
 
 
 
-        if(black_centerL == 118 && black_centerR == 10){//no line
+        if(black_centerL == 118 && black_centerR == 10){//no line flag = 0
             
+            error = 0;
             turn = avg;
+
+            flag = 0;
             
-        } else if (black_centerL == 118 && black_centerR != 10){//no left line
+        } else if (black_centerL == 118 && black_centerR != 10){//no left line turn right flag = 1
+            
+            sp = 30;
             
+            if(flag == 2){
+
+                sp = 64;
+
+            }
+
+            error = sp - black_centerR;
+            turn = middle + Kp * error;
 
-            error = 8 - black_centerR;
-            turn = Kp * error + middle;
-            flag_L = 1;
-            
+            flag = 1;
+            last_error = (1/3)*last_error + error;
         
-        } else if (black_centerL != 118 && black_centerR == 10){//no right line
+        } else if (black_centerL != 118 && black_centerR == 10){//no right line turn left flag = 2
             
-            error = 120 - black_centerL;
-            turn = Kp * error + middle;
-            flag_R = 1;
+            sp = 90;
+
+            if(flag == 1){
+
+                sp = 64;
+
+            }
+
+            error = 90 - black_centerL;
+            turn = middle + Kp * error;
             
-        } else {
+            flag = 2;
+            last_error = (1/3)*last_error + error;
+            
+        } else {//flag = 3
 
             if(60 < center && center < 68){
                 
@@ -99,22 +128,22 @@
                 turn = Kp * error + middle;
             }
 
-            if(flag_L){
+            if(flag == 1){
 
                 if(black_centerL < 100){
                     servo.set_angle(avg-0.004);
                     turn = avg;
                 }
-                flag_L = 0;
 
-            } else if(flag_R){
+            } else if(flag == 2){
 
                 if(black_centerR > 28){
                     servo.set_angle(avg+0.004);
                     turn = avg;
                 }
-                flag_R = 0;
+                
             }
+            flag = 3;
         }
 
         last_turn += turn;
@@ -129,23 +158,23 @@
         }
 
        
+        for(int i = 128; i > 64;i--){
+            if(i==64)
+                pc.printf("X");
+            else          
+                pc.printf("%c", cam.sign_line_imageL[i]);
+            }
+            
+            pc.printf("           ||             ");
 
-            for(int i = 128; i > 64;i--){
-             if(i==64)
-               pc.printf("X");
-             else          
-               pc.printf("%c", cam.sign_line_imageL[i]);
-             }
-             pc.printf("           ||             ");
-             
-             for(int i = 64; i > 0; i--){
-                 if(i==64)
-                   pc.printf("X");
-                 else          
-                   pc.printf("%c", cam.sign_line_imageR[i]);
-             }
-            pc.printf("\r\n");
-            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 = 64; i > 0; i--){
+                if(i==64)
+                    pc.printf("X");
+                else          
+                    pc.printf("%c", cam.sign_line_imageR[i]);
+            }
+        pc.printf("\r\n");
+        pc.printf("black centerL: %d   black_centerR: %d   psudo_line: %d avg: %lf flag: %d \r\n", black_centerL, black_centerR, center, avg, flag);