Diff: main.cpp

Revision:

11:9b4788de75fe

Parent:

10:e11f82b40d53

Child:

12:a3cee7619b77

--- a/main.cpp	Wed Jul 09 15:10:49 2014 +0000
+++ b/main.cpp	Thu Jul 10 06:58:37 2014 +0000
@@ -1,5 +1,4 @@
 #include "mbed.h"
-//#include "controller.h"
 #include "servo_api.h"
 #include "camera_api.h"
 #include "motor_api.h"
@@ -19,9 +18,10 @@
 int main() {
   
     double left = 0.025, middle = 0.038, right = 0.054;
-    double error, turn, last_turn = middle, avg = 0;
-    double Kp = 0.000527, Ki = 0.025;
+    double error, turn, last_turn = middle, avg = middle;
+    double Kp = 0.000327, Ki = 0.025;
     int black_centerR, black_centerL, center, times = 0;
+    int flag_R = 0, flag_L = 0;
     
     char psudo_line[128];
     
@@ -38,51 +38,55 @@
         
         black_centerL = cam.black_centerL();
         black_centerR = cam.black_centerR();
-        center = (black_centerL + black_centerR) / 2;
+        
+        //center = (black_centerL + black_centerR) / 2;
         
-        MotorA.rotate(0.2);
-        MotorB.rotate(0.2);
+        MotorA.rotate(0.15);
+        MotorB.rotate(0.15);
         
+        center = 64;
 
-        if(times % 10 == 0){
-            avg = last_turn / 10;
-            last_turn = 0;
+        if(black_centerL > 64 && black_centerR > 64){
+
+            center = (black_centerL <= black_centerR) ? black_centerL : black_centerR;
+
+        } else if(black_centerL > 64 && black_centerR < 64){
+            
+            center = (black_centerL-64 <= 64-black_centerR) ? black_centerL : black_centerR;
+
+
+        } else if(black_centerL < 64 && black_centerR > 64){
+
+            center = (64-black_centerL <= black_centerR-64) ? black_centerL : black_centerR;
+
+        } else if(black_centerL < 64 && black_centerR < 64){
+
+            center = (black_centerL >= black_centerR) ? black_centerL : black_centerR;
+
+        } else{
+
+            center = 64;
+        }
 
 
 
-            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
+        if(black_centerL == 118 && black_centerR == 10){//no line
             
             turn = avg;
             
-        } else if (black_centerL == 128 && black_centerR != 0){//no left line
+        } else if (black_centerL == 118 && black_centerR != 10){//no left line
             
 
-            /*error = 64 - black_centerR;
-            turn = Kp * error + middle;*/
-            turn = avg;
-
+            error = 8 - black_centerR;
+            turn = Kp * error + middle;
+            flag_L = 1;
             
         
-        } else if (black_centerL != 128 && black_centerR == 0){//no right line
+        } else if (black_centerL != 118 && black_centerR == 10){//no right line
             
-
-            /*error = 64 - black_centerL;
-            turn = Kp * error + middle;*/
-            turn = avg;
+            error = 120 - black_centerL;
+            turn = Kp * error + middle;
+            flag_R = 1;
             
         } else {
 
@@ -94,44 +98,71 @@
                 error = 64 - center;
                 turn = Kp * error + middle;
             }
+
+            if(flag_L){
+
+                if(black_centerL < 100){
+                    servo.set_angle(avg-0.004);
+                    turn = avg;
+                }
+                flag_L = 0;
+
+            } else if(flag_R){
+
+                if(black_centerR > 28){
+                    servo.set_angle(avg+0.004);
+                    turn = avg;
+                }
+                flag_R = 0;
+            }
         }
 
         last_turn += turn;
         times++;
-        servo.set_angle(turn);
-        
-        
-        //output the psudo map
-        for(int i = 127; i >= 0; i--)   
-            psudo_line[i] = '0';
-        
-        for(int i = center-5; i < center+5; i++)
-            psudo_line[i] = ' ';
-        
-        for(int i = 117; i > 10; i--){
-            pc.printf("%c", psudo_line[i]);    
+
+        if(times == 2){
+            
+            avg = last_turn / times;
+            servo.set_angle(avg);
+            times = last_turn = 0;
+            
         }
-        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 = 128; i > 64;i--){
+
+       
+
+            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\r\n", black_centerL, black_centerR);
-       */
+             }
+             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);
+        
+        
+        
+         //output the psudo map
+            /*for(int i = 127; i >= 0; i--)   
+                psudo_line[i] = '0';
+            
+            for(int i = center-5; i < center+5; i++)
+                psudo_line[i] = ' ';
+            
+            for(int i = 117; i > 10; i--){
+                pc.printf("%c", psudo_line[i]);    
+            }*/ 
+        
+        
+        
+       
        }