|
|
#include "common.h"
#include "include.h"
#include "find_way.h"
extern uint8 img[CAMERA_W*CAMERA_H];
extern float ago;
int Midline[60]={0},Midline_lie[80]={0},i0=0;
float deviation=0.0,q1=0.25,q2=0.75;
void xianshi(int a[],int b[],int c)
{int d,e,f;
d=a[c];
e=b[c];
f=d*0.5+e*0.5;
if(i0%2==1)
printf("%d\n",(int)f);
else
printf(" %d\n",f-e);
}
void mid_lie(int lie_right,int lie_left,int zuo_you,int a,int line2)
{
long fardeviation1=0,farnum1=0,closenum1=0,closedeviation1=0 ,closedeviation2=0;
int c,d,i,f,mid[30]={ 39,39,39,40,40,40,40, 40,40,40,40,41,41,
41,41,41,41,42,42,42,42,42,42,42,42,42,42,42,42,42},var;
for(c=(zuo_you+line2)*0.6;c>0;c--)
{ d=Midline[c];
if(abs(c-Midline_lie[d])<3) break;
}
if(lie_left==1)//顯示左列線的程序
{
for(i=59;i>c;i--)
{
Site_t site={Midline[i], i};
LCD_point(site,BLACK) ;
if(i>=55)
closedeviation2=closedeviation2+Midline[i]-mid[i/2]+a;
}
for(i=d;i<80;i++)
{ Site_t site={i, Midline_lie[i]};
LCD_point(site,BLACK) ;
}
if(closenum1!=0&&farnum1==0)
{closedeviation1=(closedeviation1/closenum1);
deviation= closedeviation2*q1+closedeviation1*q2;}
}
}
else if(lie_right==1)//顯示右列線的程序
{
{ for(i=59;i>c;i--)//行打點(diǎn)
{
Site_t site={Midline[i], i};
LCD_point(site,BLACK) ;
if(i>=55)
closedeviation1=closedeviation1+mid[i/2]-Midline[i]-a;
}
for(i=d;i>1;i--)
{
Site_t site={i, Midline_lie[i]};
LCD_point(site,BLACK) ;
f=Midline_lie[i];
}
}
var=deviation;
if(deviation<0)
var=-deviation;
Site_t site={90,40};//顯示偏差
LCD_num(site,var, BLUE, RED);
}
void lie_xunxian(int zuo_you,int erro1,int line2)
{int i,a;
for(i=Midline[zuo_you];i>3;i--)
{i=i-2;
for(a=(zuo_you+line2)*0.4;a>1;a--)
{
if(img[a*80+i]!=0&&img[(a-1)*80+i]==0)
{
Midline_lie[i]=a+erro1;
Midline_lie[i+1]=Midline_lie[i];
Midline_lie[i+2]=Midline_lie[i];
break;
}
}
}
}
void lie_xunxian_left(int zuo_you,int erro2,int line1)
{int i1,a1;
for(i1=Midline[zuo_you];i1<77;i1++)
{i1=i1+2;
for(a1=(zuo_you+line1)*0.4;a1>1;a1--)
{
if(img[a1*80+i1]!=0&&img[(a1-1)*80+i1]==0)
{
Midline_lie[i1]=a1+erro2;
Midline_lie[i1-1]=Midline_lie[i1];
Midline_lie[i1-2]=Midline_lie[i1];
break;
}
}
}
}
void buxian_left(int a[],int c[],int i,int time)
{ int b,row=a[i],d,b1,e;
float k,result;
if(i<56&&i>4&&row>10)
if(time!=1)
if(img[(i+2)*80+row]!=0&&img[(i-3)*80+row]!=0)
if(c[i]>75)
{ time=1;
for(b=i;b<56;b++)
{if((row-a[b+2])>(row-a[b+4]))break;
b++;
}
k=(float)(b-i)/(float)(a[i]-a[b]);
b1=i;
d=a[b];
for(b=row;b<79;b++)
{ result=i-k*(b-d);
if(result>0){
img[((int)result+1)*80+b]=0;
for(e=b1;e>result;e--)
{img[e*80+b]=0;
Site_t site={b, e};
LCD_point(site,BLACK) ;
}
b1=result;
Site_t site={b, result};
LCD_point(site,BLACK) ;
}}
}
}
void buxian_right(int a[],int c[],int i,int time)
{ int b,row=a[i],d,b1,e;
float k,result;
if(i<56&&i>4&&row<69)
if(time!=1)
if(img[(i+2)*80+row]!=0&&img[(i-3)*80+row]!=0)
if(c[i]<4)
{ time=1;
for(b=i;b<56;b++)
{ if((row-a[b+2])<(row-a[b+4]))break;
b++;
}
k=(float)(b-i)/(float)(a[b]-a[i]);
d=a[b];
b1=i;
for(b=row;b>0;b--)
{result=i-k*(d-b);
if(result>0){
img[((int)result+1)*80+b]=0;
for(e=b1;e>result;e--)
{img[e*80+b]=0;
Site_t site={b, e};
LCD_point(site,BLACK) ;
}
b1=result;
Site_t site={b, result};
LCD_point(site,BLACK) ;
}}
}
}
void find_edge()
{
int zuo_you=59,i=0,before_mid=40,before_mid_left,before_mid_right,use_line=1,a=0,time=0,time1=0,time2=0,time3=0;
int lie_right=0,lie_left=0,line,row,Splicing_right=0,Splicing_left=0;//line行row列
int left_edge[60]={0},right_edge[60]={0},i1;
float k,k1,erro,other_edge,deviation2[30]={9,9,9,9,9,10,11,12,13,
13,14,14,16,17,19,19,20,21,21,21,22,23,24,25,26,27,27,28,28,28};
i0++;
for(line=59;line>use_line;line--)
{ erro=deviation2[line/2];
other_edge=2*erro-3;
if(line==59)
{i1=40;
for(i=40;i<79&&i1>0;i++)
{i1--;
if(img[line*80+i]!=0)
{ before_mid=i+1;
break; }
else if(img[line*80+i1]!=0)
{ before_mid=i1;
break; } }
before_mid_right=before_mid;
before_mid_left=before_mid;
if( i==79)
break; }
else if(img[line*80+Midline[line+1]]==0)
{ use_line=line;
break; }
if(line!=59)
for(i=0;i<20&&before_mid_right>1;i++)
{
if( img[line*80+before_mid_right]!=0) break;
else before_mid_right=before_mid_right-2; }
for(row=before_mid_right;row<77;row++)
{ if(img[line*80+row]!=0&&img[line*80+row+1]==0)//右邊線檢測(cè)
{ right_edge[line]=row;
before_mid_right=right_edge[line]-2;
for(row=right_edge[line]-other_edge;row>2;row--)//left dege
{if(img[line*80+row]!=0&&img[line*80+row-1]==0)//left 邊線檢測(cè)
{ left_edge[line]=row;
zuo_you=line;
Midline[line]=(right_edge[line]+left_edge[line])/2-Splicing_left;
break;}}
if(row==2){left_edge[line]=0;
Midline[line]=right_edge[line]-erro-Splicing_left;}
//xianshi(right_edge,left_edge,line);
if(line>15&&line<51
&&img[(line-2)*80+Midline[line]]==0
&&right_edge[line+8]<72)//彎道奇化處理
{use_line=line;
lie_right=1;
lie_xunxian( zuo_you, erro,line);
a=-0.4*deviation2[line/2];
}
break; } }//右邊線檢測(cè)結(jié)束
if(row==77)
{right_edge[line]=79;
if(line!=59)
{before_mid_left=Midline[line+2]-erro;
for(i=0;i<20&&before_mid_left<79;i++)
{if( img[line*80+before_mid_left]!=0) break;
else before_mid_left=before_mid_left+2;}
}
for(row=before_mid_left;row>2;row--)
{ if(img[line*80+row]!=0&&img[line*80+row-1]==0)//左邊線檢測(cè)
{ left_edge[line]=row;
Midline[line]=left_edge[line]+erro+Splicing_right;
if(line>15&&line<51
&&img[(line-2)*80+Midline[line]]==0
&&left_edge[line+8]>7)//彎道奇化處理
{use_line=line;
lie_left=1;
lie_xunxian_left(zuo_you, erro,line);
a=0.4*deviation2[line/2];
}
break; }
else {left_edge[line]=0;
Midline[line]=47;}}//行處理結(jié)束
}//have in 到此結(jié)束
//before_mid= Midline[line];
/* if(time3==0//十字處理
&&line<46
&&line>5
&&(right_edge[line]-right_edge[line+10]<-35
||left_edge[line]-left_edge[line+10]>35)
&&left_edge[line+8]<50
&&right_edge[line+8]>74
&&img[(line-5)*80+Midline[line]]!=0)
{time3=1;
for(i=59;i>line;i--)
Midline[i]= Midline[line];
}*/
if(time1==0
&&line<57
&&left_edge[line]==0
&&left_edge[line+2]!=0
&&right_edge[line+2]!=79)//左邊使中線連接
{ time1=1;
Splicing_left=Midline[line]-Midline[line+2];
Midline[line]=Midline[line]- Splicing_left;}
else if(time1==0
&&line<57
&&right_edge[line]==79
&&right_edge[line+2]!=79
&&left_edge[line+2]!=0)//右邊使中線鏈接
{ time1=1;
Splicing_right=Midline[line+2]-Midline[line];
Midline[line]=Midline[line]+Splicing_right;
}
/* if(time2==0//環(huán)島處理
&&line<51
&&line>4
&&(right_edge[line]-left_edge[line])-(right_edge[line+2]-left_edge[line+2])>0
&&(right_edge[line+2]-left_edge[line+2])-(right_edge[line+4]-left_edge[line+4])>0
&&(right_edge[line+4]-left_edge[line+4])-(right_edge[line+6]-left_edge[line+6])>0)
if(left_edge[line]!=0
||right_edge[line]!=79 )
{if(abs(right_edge[line]-right_edge[line+4])>abs(left_edge[line]-left_edge[line+4]))
{ time2=1;
a=deviation2[line/2];
break;
Site_t site={90,40};//顯示偏差
LCD_num(site,line, BLUE, RED);}
else if(abs(right_edge[line]-right_edge[line+4])<abs(left_edge[line]-left_edge[line+4]))
{time2=1;
a=-deviation2[line/2];
break;
Site_t site={115,40};//顯示偏差
LCD_num(site,line, BLUE, RED);}}*/
buxian_left(left_edge,right_edge,line,time);
buxian_right(right_edge,left_edge,line,time);
Midline[line-1]=Midline[line] ;
if(line<56&&abs(Midline[line]-Midline[line+2])>35)//處理中線差距過大
use_line=line;
line--;
}//大for到此結(jié)束
mid_lie(lie_right,lie_left,zuo_you,a,line);
for(i=59;i>use_line;i--)//處理中線出現(xiàn)直角
{ k=(float)(i-use_line)/(float)(Midline[use_line]-Midline[i]);
k1=(float)(59-i)/(float)(Midline[i]-Midline[59]);
if( k>-0.2&&k<0&&k1>1)
{use_line=i;
break;
}
}
if(lie_left==0&&lie_right==0)
{midlinelcd(use_line+2,a);
mindlindnum(use_line+2,a);}
}
void midlinelcd(int num,int a )
{
int stander ;
uint8 i;
for(i=59;i>num;i--)
{
stander= Midline[i]+a;
Site_t site={stander, i};
LCD_point(site,BLACK) ;
}
}
void mindlindnum(int b,int a)
{ int i;
float mid[30]={ 39,39,39,40,40,40,40,
40,40,40,40,41,41,41,41,41,41,42,42,42,42,42,42,42,42,42,42,42,42,42};
float closedeviation=0.0,closedeviation1=0.0 , fardeviation=0.0,farnum=0.0,closenum=0.0;
for(i=59;i>b;i--)
{
if(i>=55&&i%2==1)
closedeviation1=closedeviation1+ Midline[i]+a-mid[i/2];
else if(i<55&&i>30&&i%2==1)
{ closedeviation=closedeviation+ Midline[i]+a-mid[i/2];
closenum=closenum+1.0 ;
}
else if(i<30&&i%2==1)
{fardeviation=fardeviation+ Midline[i]+a-mid[i/2];
farnum=farnum+1.0 ;
}
i--;
}
if(closenum!=0&&farnum==0)
{closedeviation=(closedeviation/closenum) ;
deviation=closedeviation1*q1+closedeviation*q2;
}
else
{
fardeviation=(fardeviation/farnum);
closedeviation=(closedeviation/closenum);
deviation=closedeviation1*q1+(closedeviation+fardeviation)*q2;
}
/* Site_t site={90,40};//顯示偏差
if(deviation<0)
LCD_num(site,-deviation, BLUE, RED);
else
LCD_num(site,deviation, BLUE, RED); */
}
|
評(píng)分
-
查看全部評(píng)分
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
