Revision 9:c318ee86d6e9, committed 2014-07-09

Comitter:

physicsgood

Date:

Wed Jul 09 07:34:23 2014 +0000

Parent:

8:c2c3aee85c2d

Commit message:

0709

Changed in this revision

--- a/camera_api.cpp	Tue Jul 08 04:36:09 2014 +0000
+++ b/camera_api.cpp	Wed Jul 09 07:34:23 2014 +0000
@@ -73,7 +73,7 @@
           
        //input 128 //both  
           
-          for(int i=0;i<128;i++){     
+          for(int i=127;i>=0;i--){     
               *cam_clk=1;   
               wait_us(5);
              

--- a/main.cpp	Tue Jul 08 04:36:09 2014 +0000
+++ b/main.cpp	Wed Jul 09 07:34:23 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;
@@ -23,12 +21,16 @@
     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.041, right = 0.057;
+    //double left = 0.04, middle = 0.058, right = 0.076;
+    double error, turn, last_turn =0.0;
+    double Kp = 0.00025;
+    double v = 0.3;
     int black_centerR, black_centerL, center;
     
     char psudo_line[128];
+    int times = 0;
+    double avg = middle;
     
 #ifdef Debug_cam_uart
      pc.baud(115200);
@@ -37,17 +39,28 @@
   while(1){   
      
         cam.read();
-        MotorA.rotate(0.0);
-        MotorB.rotate(0.0);
+        
         
-        black_centerL = cam.black_centerL();
+        //black_centerL = cam.black_centerL();
+        center = cam.black_centerL();
         black_centerR = cam.black_centerR();
-        center = (black_centerL + black_centerR) / 2;
+        /*int R = 64 - black_centerR;
+        int L = 64 - black_centerL;
+        if( L > 0 && R > 0){
+            center = (L >= R )? black_centerL : black_centerR;
+        }
+        else if(black_centerL < 0 && black_centerR > 0){
+            center = ((-1)*L >= R )? black_centerL : black_centerR;
+        }
+        else if(black_centerL > 0 && black_centerR < 0){
+            center = (L >= (-1)*R )? black_centerL : black_centerR;
+        }
+        else{
+            center = ((-1)*L >= (-1)*R )? black_centerL : black_centerR;
+        }*/
         
-        MotorA.rotate(0.2);
-        MotorB.rotate(0.2);
-        
-        if(black_centerL == 128 && black_centerR == 0){//no line
+        //center = (black_centerL + black_centerR) / 2; 
+        /*if(black_centerL == 128 && black_centerR == 0){//no line
             
             turn = middle;
             
@@ -69,30 +82,20 @@
                 error = 64 - center;
                 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);
-            }
-            servo.set_angle(turn);
-        } else {
-            servo.set_angle(turn);
-        }
-        last_turn = turn;*/
+        }*/
+        if(center<68 &&center>60) 
+            turn = middle;
+        else 
+            turn = middle + Kp *(64-center);
+        
         
         servo.set_angle(turn);
-        
-        
+        //MotorA.rotate(0.3);
+        //MotorB.rotate(0.3);
+        //last_turn += turn;
+        //times++;
         //output the psudo map
-        for(int i = 127; i >= 0; i--)   
+        /*for(int i = 127; i >= 0; i--)   
             psudo_line[i] = '0';
         
         for(int i = center-5; i < center+5; i++)
@@ -103,9 +106,9 @@
         }
         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);
-        
+        */
         
-        /*for(int i = 128; i > 64;i--){
+        for(int i = 128; i > 64;i--){
              if(i==64)
                pc.printf("X");
              else          
@@ -120,9 +123,10 @@
                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("black_centerL: %d   black_centerR: %d  center:  %d    turn:   %lf\r\n", black_centerL, black_centerR,center,turn);
+       
        }
+       
 
         
      

--- a/motor_api.cpp	Tue Jul 08 04:36:09 2014 +0000
+++ b/motor_api.cpp	Wed Jul 09 07:34:23 2014 +0000
@@ -64,11 +64,11 @@
          
          case 'A':
               *forward_A=level;
-              *backward_A=0;
+              *backward_A=0.1;
          break;
          case 'B':
               *forward_B=level;
-              *backward_B=0;
+              *backward_B=0.1;
          break;  
          }
            
@@ -79,11 +79,11 @@
          switch(Type){
          
          case 'A':
-             *forward_A=0;
+             *forward_A=0.1;
               *backward_A=level;
          break;
          case 'B':
-            *forward_B=0;
+            *forward_B=0.1;
               *backward_B=level;
          break;  
          }

--- a/servo_api.cpp	Tue Jul 08 04:36:09 2014 +0000
+++ b/servo_api.cpp	Wed Jul 09 07:34:23 2014 +0000
@@ -1,8 +1,8 @@
 // 0~180 angle    1~2ms
 #include "mbed.h"
 #include "servo_api.h"
-#define right_end 1.0  //90
-#define left_end 0.0  //-90
+#define right_end 0.057  //90
+#define left_end 0.025  //-90
 
 //memory opt
 // 5 degree seperate