my_wok.rar_K60循迹_k60小车循迹_k60循迹程序_wok_双线循迹

//#include "get_cenline.h" #include "common.h" //typedef unsigned char byte; /* 8 bits */ #include "get_cenline.h" # define bline_gate 5 # define bline_space 40 # define pixrelative_gate 20 # define ROW 60 # define COL 120 # define blabs_gate 100 # define lspace_gate 15 # define show_line_gate 3 extern byte camer_data[ROW][COL]; extern byte cenline[ROW]; extern byte cenline_copy[ROW]; //extern byte lp_pos; extern byte lcenline; extern byte row_valid; extern byte turn; //extern byte cross_bit; //extern byte state; /*extern byte l_out; extern byte r_out; extern byte l_outofline; extern byte r_outofline; */ extern struct black_segment { byte len; byte pos; byte pix_value; }bline[4]; //byte cenline[ROW]; // byte cenline_copy[ROW]; //int pulse_num; //word estimatedSpeed; // byte start_flag; byte mid(byte a,byte b,byte c) { if(a>=b && b>=c|| a<=b && b<=c) return b; else if( a>=c && c>=b|| a<=c && c<=b) return c; else return a; } byte max( byte a,byte b) { if(a>98) return b; else if(b>98) return a; else if(a<b) return b; else return a; } byte min(byte a,byte b) { if(a>98) return b; else if(b>98) return a; else if(a<b) return a; else return b; } byte abs_2( byte a,byte b) { if(a>b) return a-b; else return b-a; } byte div_2(byte a) { byte b; b=a>>1; return b; } void medianfilter( void) { byte i,j; for (i=0;i<ROW;i++) for(j=0;j<COL;j++) { if(camer_data[i][j]<80&&i<25) continue; else { if(camer_data[i][j]>200) { camer_data[i][j]=200; } else if(camer_data[i][j]<30) { camer_data[i][j]=30; } if(j==0) camer_data[i][j]=mid(camer_data[i][j],camer_data[i][j+1],camer_data[i][j+2]); else if(j==COL-1) camer_data[i][j]=mid(camer_data[i][j],camer_data[i][j-1],camer_data[i][j-2]); else camer_data[i][j]=mid(camer_data[i][j-1],camer_data[i][j],camer_data[i][j+1]); } } } void GetCenLine_1(void) { //-------------------------全局变量---------------------------------------// byte cross_bit=0; //------------------------------------------------------------------------// byte d_g=0; //下降沿标志 byte r_g=0; //上升沿标志 int cnt=0; const byte dis=3; //边缘差值间隔 const byte one=1; const byte line_gate=15; byte lline_pos[ROW]; //左边边线位置数组 byte rline_pos[ROW]; //右边边线位置数组 //byte rowbit[ROW]; byte l_out=0; byte r_out=0; //byte l_max=3; byte relative_gate=55; //边缘检测阈值 byte row; //行变量 byte col; //列变量 //byte l_outline; //byte r_outline; byte l_outofline=0; byte r_outofline=0; byte linetmp[3]; byte rlinetmp[3]; byte i=0; int delta; byte lstart_pos; byte tmpk; byte tmpk1; byte cnttmp; byte show_lline=0; byte show_rline=0; byte show_sl=0; byte show_rl=0; int tmp_col; int sc; byte l_out_row=0; byte l_out_col=0; byte l_out_cnt=0; byte r_out_row=0; byte r_out_col=0; byte r_out_cnt=0; float delta_x; byte delta_y; float c_delta; byte state=0;; byte oncel=0; byte oncer=0; byte tr_row; int ss; //byte last_lline[6]; //byte last_rline[6]; byte error=0; int range; int rangel; int ranger; byte rrow; byte l_flag; byte r_flag; //初始化 byte cross_flag=0; turn=0; row_valid=0; cross_flag=0; l_out_row=0; l_out_col=0; l_out_cnt=0; r_out_row=0; r_out_col=0; r_out_cnt=0; // medianfilter(); for(row=ROW-1;row>ROW-7;row--) { cnt=0; d_g=0; r_g=0; l_flag=0; r_flag=0; oncel=0; oncer=0; //once=0; cenline[row]=50; for(i=0;i<5;i++) //初始化记录每行黑线的结构体 { bline[i].len=0; bline[i].pos=0; bline[i].pix_value=255; } lline_pos[row]=1; rline_pos[row]=COL-2; if (row<line_gate) //边缘阈值与行变量的关系 relative_gate=70; else relative_gate=70; //camer_data[row][max(2-6,1)]=0; for(col=0;col<COL-3;col++) { delta=(int)(camer_data[row][col+dis]-camer_data[row][col]); if(delta <= (-(int)relative_gate)) //检测到黑线下降沿 { if(d_g==0) //锁存黑线其实位置记为 lstart_pos 并记录黑线的像素值 { lstart_pos=col+dis; d_g=1; r_g=0; } else { if(col+dis-lstart_pos>bline_gate) { bline[cnt].len=2; bline[cnt].pos=lstart_pos+1; cnt++; lstart_pos=col+dis; d_g=1; r_g=0; } } } else if(delta>=relative_gate) //检测到黑线上升沿 { if(d_g==0) { lstart_pos=col; d_g=1; r_g=0; } if(r_g==0) { bline[cnt].len=col-lstart_pos+1; bline[cnt].pos=div_2(lstart_pos+col); cnt++; r_g=1; } else if (r_g==1) { cnt--; bline[cnt].len=col-lstart_pos+1; bline[cnt].pos=div_2(lstart_pos+col); cnt++; } if(col+4<COL) { if(camer_data[row][col+4]-camer_data[row][col+1]<relative_gate) { d_g=0; r_g=0; } } else { d_g=0; r_g=0; } } } if(d_g==1) { bline[cnt].len=1; bline[cnt].pos=lstart_pos+one; cnt++; } for(cnttmp=0;cnttmp<cnt;cnttmp++) { if(bline[cnttmp].pos<50&&bline[cnttmp].pos>0) { lline_pos[row]=bline[cnttmp].pos; l_flag=1; } else if(bline[cnttmp].pos>70&&bline[cnttmp].pos<COL-2) { if(rline_pos[row]!=COL-2) continue; else { r_flag=1; rline_pos[row]=bline[cnttmp].pos; } } } if(l_flag==0) { if(row==ROW-1) lline_pos[row]=1; else lline_pos[row]=lline_pos[row+1]; } if(r_flag==0) { if(row==ROW-1) rline_pos[row]=COL-2; else rline_pos[row]=rline_pos[row+1]; } /* if(abs_2(lline_pos[row],lline_pos[row+1])>10) { lline_pos[row]=lline_pos[row+1]; } if(abs_2(rline_pos[row],rline_pos[row+1])>10) { rline_pos[row]=rline_pos[row+1]; } */ } for(row=ROW-7;row<ROW;row--) { cnt=0; d_g=0; r_g=0; lstart_pos=0; lline_pos[row]=1; //rline_pos[row]=98; cenline[row]=50; if(error==1) { lline_pos[row]=lline_pos[row+1]; rline_pos[row]=rline_pos[row+1]; continue; } for(i=0;i<5;i++) //初始化记录每行黑线的结构体 { bline[i].len=0; bline[i].pos=0; bline[i].pix_value=255; } /*if(oncel==1) camer_data[row][66]=0; if(oncer==1) camer_data[row][67]=0; /* if(l_out==1||r_out==1) { if(state!=5) { if(l_out==1&&r_out!=1) st