123

Dependencies:   mbed

Fork of Boboobooo by Clark Lin

main.cpp

Committer:
backman
Date:
2014-06-03
Revision:
1:82bc25a7b68b
Parent:
0:68c173249c01
Child:
2:c51647d3c14d

File content as of revision 1:82bc25a7b68b:

#include "mbed.h"
#include "servo_api.h"
#include "camera_api.h"
//BX_servo servo;


Serial pc(USBTX, USBRX);
BX_servo servo; 
 
    BX_camera cam('n');
    PwmOut cam_clk(PTC3);

int main() {
    
  /*  
    int black_va;
    int white_va;
    */
    
    
     pc.baud(115200);
     
     
     
     
     char find_type=0x00;
     
     int b_end;
     int b_start;
     
     int b2_end;
     int b2_start;
     
     
     int center;
     int j=64;
     bool f1;
     bool f2;
     bool f3;
     bool f4;
     while(1){
         
         
         cam.read();
         
        for(int i=0;i<128;i++){
             if(i==64)
               pc.printf("X");
             else          
               pc.printf("%c", cam.sign_line_image[i]);
         }
         pc.printf("\r\n");
         
       
       
       
         // find center 
         // case 1      //  |  //
         //case 2         / |  /
         
            if(cam.sign_line_image[64]==' ')
              find_type=0x02;
            else
              find_type=0x01;  
            
            b_start=0;
            b_end=0;   
            b2_start=0;
            b2_end=0;
            
            j=64;
            f1=false;
            f2=false;
            f3=false;
            f4=false;
           for(int i=64;i<128; i++,j--){
               
                
               switch(find_type){
                   
                   
                   case 0x01:
                       
                         if(f1==false&&cam.sign_line_image[i]==' '){ 
                                  if(f1==false){
                                      b_start=i;
                                      f1=true;
                                  }
                                  
                         }
                         if(f1== true&& f2==false&&cam.sign_line_image[i]=='O'){ 
                                  if(f2==false){
                                      b_end=i-1;
                                      f2=true;
                                  }
                                  
                         }
                         
                         if(f3==false&&cam.sign_line_image[j]==' '){ 
                                  if(f3==false){
                                      b2_end=j;
                                      f3=true;
                                  }
                                  
                         }
                         if(f3==true&&f4==false&&cam.sign_line_image[j]=='O'){ 
                                  if(f4==false){
                                      b2_start=j-1;
                                      f4=true;
                                  }
                                  
                         }
                         
                         
                         
                       
                       
                       
                   
                   
                   break;
                   
                   
                   case 0x02:
                   
                         if(cam.sign_line_image[i]=='O'){         
                    
                                  if(f1==false){
                                      b_end=i;
                                      f1=true;
                                  }
                         }  
                    
                         if(cam.sign_line_image[j]=='O'){
                        
                                   if(f2==false){
                                       b_start=j;     
                                      f2=true;
                                  }
       
                        }
                   
                   
                   break;    
                   
                   
                   
                   
               }
        
               
           }
       
            
          switch(find_type){
         
             case 0x01:
                
               if((b_end-b_start)>(b2_end-b2_start)    )
                     center=(b_end+b_start)/2;
                     
               else  
                     center=(b2_end+b2_start)/2;
              
                pc.printf("%d %d | %d  %d\r\n",b_start,b_end,b2_start,b2_end);
             
             
             break;
             
             case 0x02:
               center=(b_end+b_start)/2;
              
             break;
         }
           pc.printf("center :%d\r\n",center);
   //--------------------------------------------            
               
       
       
         
         }
     
     
     
       
     
     
     
     
     
     
     
     
     
     
     
     
     
     //tmp
     int angle;
     float black_pR=0;
     float black_pL=0;
    float black_pT=0;
     //tune
/*      
     int min_va=90000;
     int max_va=0;
     int mid_va=0;
  */
  
  
    
    while(1){
         black_pR=0;
         black_pL=0;
         black_pT=0;
  //       min_va=90000;
    //     max_va=0;
         angle=0;
         cam.read();
          for(int i=0;i<64;i++){
                 
                 
                // angle+=cam.line_image[i]-cam.line_image[127-i];
                 if(cam.line_image[i]==0)
                     black_pR++;
                 if(cam.line_image[127-i]==0)
                    black_pL++;      
             }
             
             
             black_pT=black_pL+black_pR;
                 
               
               if(black_pL>black_pT/2){
               
                 angle=(black_pR/black_pT)*90;
                
                  //angle=
               }
               else if(black_pR>black_pT/2){
                     
                     angle=-1.0*(black_pR/black_pT)*90;
                      //angle=-0.5;
               }
               else{
                   
                 angle=0;
                   
               }
                servo.set_angle(angle);
                pc.printf("ang: %d",angle);
             pc.printf("\r\n");
               
               
               
               
                     
                 
                 
            }
             
             
             
             
             
            
             
            // pc.printf("mid: %d|min: %d | max: %d | diff: %d",mid_va,min_va,max_va,max_va-min_va);
             
            
             
             
             
           //p - controller
             
           //servo angle not yet contorl  
             
             
             
             
             
             
             
             
             
             
             
             
             
             
             
             
             
  
    
    
    
  
        
        
          
          
    
    
    
    
    return 0;
    
    
}