Revision 20:4ed21397e775, committed 2014-06-29

Comitter:

TonyLin

Date:

Sun Jun 29 16:20:22 2014 +0000

Parent:

19:eb0552a0ddae

Commit message:

6/30

Changed in this revision

--- a/camera_api.cpp	Sun Jun 29 14:02:37 2014 +0000
+++ b/camera_api.cpp	Sun Jun 29 16:20:22 2014 +0000
@@ -1,367 +1,167 @@
 #include "mbed.h"
 #include "camera_api.h"
+ 
 #define clk 2  //ms
-
-BX_camera::BX_camera(void)
+ 
+BX_camera::BX_camera(int p)
 {
-
+ 
     line_CamR = new FastAnalogIn(PTD5);
     line_CamL=   new FastAnalogIn(PTD6,0);
     cam_clk=new DigitalOut(PTE1);
     si=new DigitalOut(PTD7);
-
+    padding = p;
 }
-
+ 
 int BX_camera::black_centerR(void)
 {
-
-    int r_care=10;
-    int l_care=118;
-
-
-    // find center
-    // case 1      //  |  //
-    // case 2       /  |  /
-
-    char find_type=0x00;
-
-    int b_end=64;
-    int b_start=64;
-    int b_w=0;
-    int b_center=64;    
-    int b2_end=64;
-    int b2_start=64;
-    int b2_center=64;
-    int b2_w=0;
-    int center=30;
-    int j=64,i;
-    int offset=64;
-    bool f1=false;
-    bool f2=false;
-    bool f3=false;
-    bool f4=false;
-    bool count=false;
-    int same_chars=0;
-    int w_thr_up=32;
-    int w_thr_dn=2;
-
-    b_start=b_center=b_end=b2_start=b2_center=b2_end=offset;
-    
-    for(i=l_care-1; i>r_care; i--){
-        if(sign_line_imageR[i]==' ' && count==false){
-            b_start=i;
-            count=true;
-            if(last_sign_line_imageR[i]==' ')
-                same_chars++;
-        }
-        else if(sign_line_imageR[i]==' ' && count==true){
-            if(last_sign_line_imageR[i]==' ')
-                same_chars++;
-        }
-        else if(sign_line_imageR[i]!=' ' && count==true){
-            count=false;
-            if(same_chars>5 && same_chars<32){
-                b_end=i+1;
-                break;    
-            }
-        }
-    }
-    
-    center=(b_end+b_start)/2;
-    
-    for(i=l_care-1; i>r_care; i--){
-        last_sign_line_imageR[i]=sign_line_imageR[i];
-        }
-        
-    return center;
-/*
+ 
     int l_care=10;
     int r_care=118;
-
-
-    // find center
-    // case 1      //  |  //
-    // case 2       /  |  /
-
-    char find_type=0x00;
-
-    int b_end=64;
-    int b_start=64;
+    int b_start=0;
+    int b_end=0;
+    bool l_f1=false;
+    bool l_f2=false;
+    bool find=false;
+    int b_thr_up=32;
+    int b_thr_dn=5;
     int b_w=0;
-    int b_center=64;    
-    int b2_end=64;
-    int b2_start=64;
-    int b2_center=64;
-    int b2_w=0;
-    int center=30;
-    int j=64;
-    int offset=64;
-    bool f1=false;
-    bool f2=false;
-    bool f3=false;
-    bool f4=false;
-    int w_thr_up=32;
-    int w_thr_dn=2;
-
-    b_start=b_center=b_end=b2_start=b2_center=b2_end=offset;
-    
-    if(sign_line_imageR[64]==' ')
-        find_type=0x02;
-    else
-        find_type=0x01;
-
-
-    for(int i=64; i<r_care; i++,j--) {
-        
-        switch(find_type) {
-            case 0x01:
-                if(f1==false && sign_line_imageR[i]==' ') {
-                        b_start=i;
-                        f1=true;
-                }
-                if(f1== true && f2==false && sign_line_imageR[i]=='O') {
-                        b_end=i-1;
-                        f2=true;
-                }
-                if(f3==false && sign_line_imageR[j]==' ') {
-                        b2_start=j;
-                        f3=true;
-                }
-                if(f3==true && f4==false && sign_line_imageR[j]=='O') {
-                        b2_end=j+1;
-                        f4=true;
-                }
-                break;
-
-            case 0x02:
-                if(sign_line_imageR[i]=='O') {
-                    if(f1==false) {
-                        b_end=i-1;
-                        f1=true;
-                    }
-                }
-                if(sign_line_imageR[j]=='O') {
-                    if(f2==false) {
-                        b_start=j+1;
-                        f2=true;
-                    }
-                }
-                break;
-        }
-    }
-
-    b_w=b_end-b_start+1;
-    b2_w=b2_start-b2_end+1;
-
-    de_v=b_start;
-    de_v2=b_end;
-
-    switch(find_type) {
-        case 0x01:
-            b_center=(b_end+b_start)/2;
-            b2_center=(b2_end+b2_start)/2;
-
-            if(w_thr_up>b_w && w_thr_dn<b_w && (b_center!=offset) && (b_center-offset)<(offset-b2_center))
-                center=b_center;
-            else if(w_thr_up>b2_w && w_thr_dn<b2_w && (b2_center!=offset)) 
-                center=b2_center;
-            else
-                center=offset;
-            break;
-
-        case 0x02:
-            center=(b_end+b_start)/2;
-            break;
-    }
     
-    return center;
-    */
-}
-
-
-
-int BX_camera::black_centerL(void)
-{
-
-    int l_care=10;
-    int r_care=118;
-    // find center
-    // case 1      //  |  //
-    // case 2       /  |  /
-    char find_type=0x00;
-    int b_end=118;
-    int b_start=118;
-    int b_w=0;
-    int b_center=0;
-    int b2_end=0;
-    int b2_start=0;
-    int b2_center=0;
-    int b2_w=0;
-    int center=30;
-    int j=64;
-    bool f1=false;
-    bool f2=false;
-    bool f3=false;
-    bool f4=false;
-    int w_thr_up=32;
-    int w_thr_dn=0;
-
-    if(sign_line_imageR[64]==' ')
-        find_type=0x02;
-    else
-        find_type=0x01;
-
-
-    for(int i=64; i<r_care; i++,j--) {
-        
-        switch(find_type) {
-            case 0x01:
-                if(f1==false&&sign_line_imageR[i]==' ') {
-                    if(f1==false) {
-                        b_start=i;
-                        f1=true;
-                    }
-
-                }
-                if(f1== true&& f2==false&&sign_line_imageR[i]=='O') {
-                    if(f2==false) {
-                        b_end=i-1;
-                        f2=true;
-                    }
-
-                }
-                if(f3==false&&sign_line_imageR[j]==' ') {
-                    if(f3==false) {
-                        b2_end=j;
-                        f3=true;
-                    }
-
+     for(int i=r_care;i>l_care;i--){
+         
+         
+          if(l_f1==false&&sign_line_imageR[i]==' '){
+               
+              b_start=i;
+              l_f1=true;
+          }
+          if(l_f1==true && sign_line_imageR[i]=='O'){
+              b_end=i-1;
+              l_f2=true;   
+           }
+         
+           if(l_f1==true && l_f2== true){
+               b_w=b_start-b_end;
+               if( b_thr_up>b_w&&b_w> b_thr_dn){
+                   
+                   find=true;     
+                   break;
                 }
-                if(f3==true&&f4==false&&sign_line_imageR[j]=='O') {
-                    if(f4==false) {
-                        b2_start=j-1;
-                        f4=true;
-                    }
-
-                }
-                break;
+                else{
+                   l_f1=false;
+                   l_f2=false;
+                   
+                } 
                 
-            case 0x02:
-                if(sign_line_imageR[i]=='O') {
-                    if(f1==false) {
-                        b_end=i;
-                        f1=true;
-                    }
-                }
-                if(sign_line_imageR[j]=='O') {
-                    if(f2==false) {
-                        b_start=j;
-                        f2=true;
-                    }
-                }
-                break;
-        }
+                   
+            }
+               
+               
     }
-
-    b_w=b_start-b_end;
-    b2_w=b2_start-b2_end;
-
-    de_v=b_start;
-    de_v2=b_end;
-
-    switch(find_type) {
-        case 0x01:
-            b_center=(b_end+b_start)/2;
-            b2_center=(b2_end+b2_start)/2;
-            if(w_thr_up>b_w&&(b_center!=0)&&(b_center-64)<(64-b2_center))
-                center=b_center;
-            else
-                center=b2_center;
-            /*            if( ( w_thr_up- (b_w))>0  &&( ( w_thr_up- (b_w)) < (w_thr_up-(b2_w)) )           ) {
-                            center=(b_end+b_start)/2;
-
-
-
-              //          } else if( ( w_thr_up- (b2_w) )>0 ) {
-                            center=(b2_end+b2_start)/2;
-            //            } else {
-
-                            center=65;
-
-                            //????????????????
-
-                        }
-
-            */
-            break;
-
-        case 0x02:
-            center=(b_end+b_start)/2;
-            break;
-    }
-    return center;
-}
-
+         
+         
+         
+     
+ 
+  if(find)
+     return (b_start+b_end)/2;
+  else
+     return -1;  
+}   
+ 
+ 
+ 
+ 
+ 
 void BX_camera::read(void)
 {
-
+ 
     w_f_vL=0x0000;
     b_f_vL=0xffff;
+ 
     w_f_vR=0x0000;
     b_f_vR=0xffff;
-
+ 
+ 
+ 
+ 
     *si=1;
     *cam_clk=1;
-
+ 
     wait_us(30);   // tune here
     *si=0;
     *cam_clk=0;
-
+ 
+ 
+ 
     line_CamR->enable();
     line_CamL->enable();
-    
+ 
+ 
     //input 128 //both
+ 
     for(int i=0; i<128; i++) {
         *cam_clk=1;
         wait_us(5);
-
+ 
+ 
         line_imageR[i]=line_CamR->read_u16();
         line_imageL[i]=line_CamL->read_u16();
-
+ 
         //  big small
         if(line_imageR[i] > w_f_vR)
             w_f_vR=line_imageR[i];
         else if(line_imageR[i] < b_f_vR )
             b_f_vR = line_imageR[i];
-
-
+ 
+ 
         if(line_imageL[i] > w_f_vL)
             w_f_vL=line_imageL[i];
         else if(line_imageL[i] < b_f_vL )
             b_f_vL = line_imageL[i];
-
+ 
+ 
+ 
+ 
         *cam_clk=0;
         wait_us(5);
+ 
+ 
     }
-
+ 
+ 
     line_CamR->enable();
     line_CamL->enable();
-
+ 
+ 
     //filter L R   //may change
+ 
     for(int i=0; i<128; i++) {
+ 
+ 
         if( (line_imageR[i]-b_f_vR) < (w_f_vR - line_imageR[i] )    )
             sign_line_imageR[i]=' ';
         else
             sign_line_imageR[i]='O';
-
+ 
+ 
         if( (line_imageL[i]-b_f_vL) < (w_f_vL - line_imageL[i] )    )
             sign_line_imageL[i]=' ';
         else
             sign_line_imageL[i]='O';
-
+ 
+ 
+ 
+ 
+ 
         if(i==0) {
             sign_line_imageR[i]='X';
             sign_line_imageL[i]='X';
         }
+ 
+ 
     }
+ 
+ 
+ 
 }
\ No newline at end of file

--- a/camera_api.h	Sun Jun 29 14:02:37 2014 +0000
+++ b/camera_api.h	Sun Jun 29 16:20:22 2014 +0000
@@ -1,32 +1,54 @@
 #include "bx-adc.h"
 #include "mbed.h"
-
-
+ 
+ 
 //cause same si and clk  camR ,camL are synchronous
-
+ 
 class BX_camera{
-
-public:
-    //static
-    void read(void); //block in here
-    BX_camera();
+    
+  public:
+    
+  //static  
+     void read(void); //block in here
+    
+     
+    
+    
+    BX_camera(int p);
+    
+    
     int  black_centerR(void); //index 0~127
-    int  black_centerL(void);
+    int  black_centerL(void); 
+    
+    
     char sign_line_imageL[128];
-    char sign_line_imageR[128], last_sign_line_imageR[128];
+    char sign_line_imageR[128];
+    
     int de_v;
     int de_v2;
     
-private:
-    FastAnalogIn* line_CamR;
-    FastAnalogIn* line_CamL;
-    DigitalOut* cam_clk;
-    DigitalOut* si;
-    unsigned int line_imageR[128];   //may buffer
-    unsigned int line_imageL[128];
-    int w_f_vL;
-    int b_f_vL;
-    int w_f_vR;
-    int b_f_vR;
-
-};
\ No newline at end of file
+    
+  private:
+    
+           FastAnalogIn* line_CamR;
+           FastAnalogIn* line_CamL;  
+           DigitalOut* cam_clk;
+           DigitalOut* si;
+           
+           unsigned int line_imageR[128];   //may buffer
+           unsigned int line_imageL[128];
+           
+         
+          int w_f_vL;
+          int b_f_vL;
+   
+          int w_f_vR;
+          int b_f_vR;
+    
+    
+          int padding;
+ 
+    
+    
+    
+    };
\ No newline at end of file

--- a/main.cpp	Sun Jun 29 14:02:37 2014 +0000
+++ b/main.cpp	Sun Jun 29 16:20:22 2014 +0000
@@ -16,7 +16,7 @@
 
 Serial pc(USBTX, USBRX);
 BX_servo servo;
-BX_camera cam;
+BX_camera cam(0);
 BX_motor MotorA('A');
 BX_motor MotorB('B');
 BX_pot pot1('1');            // 90/30=3
@@ -85,13 +85,17 @@
 
 #ifdef motor_on
         motor=pot1.read();
-        MotorA.rotate(0.0);
-        MotorB.rotate(0.0);
+        MotorA.rotate(motor);
+        MotorB.rotate(motor);
+        //pc.printf("%f\r\n",motor);
 #endif
 
         b_r_c=(double)cam.black_centerR();
         //pc.printf("%d %d speed :%f  bk_center %f PID:%f \r\n",cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
-
+        if(b_r_c<0.0){
+            pc.printf("\r\n");
+            continue;
+            }
         //compute
         //stand at 65
         error=b_r_c-offset;
@@ -113,7 +117,7 @@
                 r_kp*=-1.0;
                 r_kp_neg=true;
             }
-        kp=0.016/(30-(r_kp/10.0));
+        kp=0.016/(50-(r_kp/10.0));
         if(r_kp_neg==true){
                 r_kp_neg=false;
                 r_kp*=-1.0;

--- a/motor_api.cpp	Sun Jun 29 14:02:37 2014 +0000
+++ b/motor_api.cpp	Sun Jun 29 16:20:22 2014 +0000
@@ -2,20 +2,7 @@
 #include "motor_api.h"
 
 #define init_v 10  // period in 1.35 ~170ms
-
-
-
-
-
-
-
-
  BX_motor::BX_motor(char type){
-    
-    
- 
- 
-  
    //need N level????
   engine_enable = new DigitalOut(PTE21);