前言
在我比赛的算法，通常在处理元素之前，需要具备良好的图像边界，拐点，边界起始行，边界截止行等主要条件。之前系列文章介绍过了边界，和拐点。有人留言提到过拐点的程序，其实最简单的方法，就是连续几行的边界做差，差值满足条件就判断为拐点。
圆环！！这个在我做车三年一直都是噩梦存在的元素，虽然16届增加了新元素三岔，我依然在圆环花费了最多的时间。因为今年选用的是摄像头入环，所以在处理圆环的时候，不仅仅要考虑如何识别，还需要在入环，环内进行补线处理。于是我在处理圆环大致分为识别，入环切线，入环 ，环内，出环，出环切线，完全出环这几个阶段。
void R_uppoint( unsigned char Row_START,unsigned char Row_END ,unsigned char Col_START,unsigned char Col_END,unsigned char Edge_ROW)
{
int Col;
//uint8 N_P=0;
int C1=0;
int Row=Edge_ROW;
for(Row=Edge_ROW;Row>Edge_ROW-7 && Row>ROW_START+1;Row--)//从本行开始向上扫10行
{
for(Col=Col_END-2;Col>15;Col--)//初始值固定
{
if(difference_sum(image[Row][Col-COL_SPACE],image[Row][Col])>=g_threshold_value)//检测到跳变点
{
if( abs(Col-COL_SPACE-C1)<2&& Col-COL_SPACE<=C1)//如果两列差值小于3，说明C为拐点
{
Inflection_point.R_up_point.x=Row;
Inflection_point.R_up_point.y=C1;
return;
}
C1=Col-COL_SPACE;
break;
}
}
}
}
一、起始行和截止行
顾名思义吧，起始行就是第一次扫到边界的那一行，截止行是最后一次找到边界的行。这两个特征代表了边界的可用范围。
二、圆环识别：
圆环的特征还是相当明显，一边边线，连续，一边边线，缺失。这里我采用的方式是在识别到一边拐点存在，一边拐点不存在，并且存在的拐点距离最远端很近，这里我以当拐点距离最远行小于5行为标准。当存在这些条件后，再进行拐点上面几行边线缺失判断。以左环为例，当左边存在拐点，并且拐点上方有三行不存在边线。就行下面的右边线计数，当右边先满足远小近大的条件，并且连续存在13行边线就满足圆环的识别。这种识别是很稳定的，但由于车库的存在通常会与车库误识别，所以对于两者区别还需要想办法解决。
round->Entrance_count=0;//圆环入口计数清零
if(point->L_down_point.x+5>ROW_END-1)
Round_row=ROW_END-1;
else
Round_row=point->L_down_point.x+5;
for(;Round_row>point->L_down_point.x-10&&Round_row>=ROW_START;Round_row--)//从左拐点下面5行到上面10行进行搜索
{
if(str->RightEdge[Round_row+1]!=0&&str->RightEdge[Round_row]!=0)
{
//如果右边正常递减，且不丢线 斜率符合要求
if(str->RightEdge[Round_row+1]-str->RightEdge[Round_row]>=0 &&str->RightEdge[Round_row+1]-str->RightEdge[Round_row]<=2)
{
round->Entrance_count++; //圆环入口计数增加
}
}
}
if(round->Entrance_count>=13)//min修改，其中有13行满足条件即可判定为圆环
{
round->Round_flag=1;
RoundType=Left_Round;
RoundProcess=Find_Round;
RoadType= Round_Road;
// BlueRing;
//RoundSize=Large_Round; //待定
}进环前处理：
我们在识别到环的时候，是在环外的，所以需要保持一段距离直行到环的切点A。我的方法是通过 B点之前的斜率进行补线。 
for(Round_row=ROW_END;Round_row>=ROW_START;Round_row--)
{
if(str->RightEdge[Round_row])
{
char temp_r;
char temp_l;
char track;
temp_r=str->RightEdge[Round_row];
track=Boundary.track_Width[Round_row];
temp_l=temp_r-track-1;
if(temp_l>=79)
{
temp_l=78;
}
if(temp_l<=1)
{
temp_l=1;
}
str->LeftEdge[Round_row]=temp_l;
// str->LeftEdge[Round_row]=str->RightEdge[Round_row]-str->track_Width[Round_row];//左边界等于右边界减半宽
}
else
{
str->LeftEdge[Round_row]=0;
}
}进环处理：
对于进环，需要将右边线通过补线方式补成一个类似与弯道的情况。然后通过陀螺仪积分判断旋转的角度，当角度满足车身已经进入到环里的条件，就可以正常的巡线走了。
case Into_Round://进环 关键 分两步
{
// lcd_showint8(110,6, 2);
/*进环的补线处理*/
if(!Inflection_point.L_down_point.x&&!Inflection_point.L_up_point.x&&str->LeftEdge[ROW_END-5])//&&str->LeftEdge[ROW-4])
// if(Inflection_point.L_up_point.x==0&&Inflection_point.L_down_point.x&&str->LeftEdge[ROW_END-3])
// if(Inflection_point.L_up_point.x&&(left_ad>=220||right_ad>200)&&!str->LeftEdge[ROW_END-1])
{
RoundProcess=Into_Round2;
}
else
{ //获取补线点信息
if(point->L_down_point.x>5) //记录上一次拐点存在的信息
{
Inflection_Temp_x=point->L_down_point.x;
Inflection_Temp_y=point->L_down_point.y;
}
//得到信息后，开始补线
for(uint8 i=Inflection_Temp_x;i>2;i--)
{
if(image[i][Inflection_Temp_y]>=Threshold&&image[i-1][Inflection_Temp_y]<Threshold)
{
LeftLoopTemp_R=i-1;
str->EndLine=i-1;
break;
}
}
for(uint8 i=Inflection_Temp_y;i<75;i++)
{
if((image[LeftLoopTemp_R-1][i+1]>=Threshold)&&(image[LeftLoopTemp_R-1][i]<Threshold))
{
LeftLoopTemp_C=i;
break;
}
}
/*进环斜率*/
LeftLoopSlope=1.3;//*(str->RightEdge[aa1]-aa2)/(aa1-LeftLoopTemp_R);
for(uint8 i=0;i<=28;i++)
{
str->RightEdge[LeftLoopTemp_R+i]=(int)(i*LeftLoopSlope+LeftLoopTemp_C);
str->RightEdge[LeftLoopTemp_R+i]=int8_range_protect(str->RightEdge[LeftLoopTemp_R+i],1,79) ;
if(str->RightEdge[LeftLoopTemp_R+i]>78||LeftLoopTemp_R+i>ROW_END)
break;
}
}
for(uint8 i=str->EndLine;i<ROW_END;i++)
{
for(uint8 j=1;j<=79;j++)
{
if(image[i][j]>=Threshold&&image[i][j+1]<Threshold)
{
if(str->RightEdge[i]>j+1)
str->RightEdge[i]=j+1;
break;
}
}
}
break;
}
case Into_Round2://进环 关键 4
{
// lcd_showint8(110,6, 3);
/*判断是否进入环岛*/
if(angle_z<=-40)//&&str->L_StartLine==0&& RoundSize==Small_Round) //定距离一段
{
RoundProcess=In_Round;
Inflection_Temp_x=0;
Inflection_Temp_y=0;
//lcd_showint8(80,5, 4);
}
#if 1 //补线
for(uint8 i=str->EndLine;i<=ROW_END;i++)
{
for(uint8 j=1;j<=75;j++)
{
if(image[i][j]>=Threshold&&image[i][j+1]<Threshold&&image[i][j+2]<Threshold&&image[i][j+3]<Threshold)
//if (difference_sum(image[i][j],image[i][j-COL_SPACE])>=g_threshold_value)
{
if(str->RightEdge[i]>j+1)
str->RightEdge[i]=j+1;
break;
}
}
}
/*进环斜率*/
LeftLoopSlope=2.0;
for(uint8 i=ROW_END;i>ROW_START;i--)
{
// lcd_showint8(80,5, 5);
if(str->RightEdge[i]!=0)
str->RightEdge[i]=str->RightEdge[i]<(int)(LeftLoopSlope*i+5)?str->RightEdge[i]:(int)(LeftLoopSlope*i+5);
else
str->RightEdge[i]=(int)(LeftLoopSlope*i+5);
if(str->RightEdge[i]>79)
str->RightEdge[i]=0;
}
for(uint8 i=29;i>str->EndLine&&i>ROW_START+2;i--)
{
//lcd_showint8(80,5, 6);
if(image[i][10]>=Threshold&&image[i-1][10]<Threshold&&image[i-2][10]<Threshold)
//if (difference_sum(image[i-2][10],image[i][10])>=g_threshold_value)
{
for(uint8 j=i;j>str->EndLine&&j>ROW_START;j--)
{
str->LeftEdge[j]=0;
}
str->EndLine=i-1; //改变endline
break;
}
if(str->LeftEdge[i]>30)
{
str->LeftEdge[i]=0; //限幅
}
}
#endif
break;
}出环处理：
在出环时，由于左边线不存在，因此也需要进行补线操作。目的就是为了出环时车转弯方向是正确的，
case Leave_Round://离开圆环
{
// lcd_showint8(110,4, 6);
Round_num=0;
for(Round_row=str->R_StartLine;Round_row>str->EndLine+1&&Round_row>ROW_START+10;Round_row--)
{
if(str->RightEdge[Round_row]==0)
{
Round_num++;
}
}
if(Round_num<3&&((str->R_StartLine-str->EndLine)>22||str->RightEdge[str->EndLine]>18)) //30需要调 huo 判断斜率
{
RoundProcess=Leave_Round2;//暂定
}
else
{
LeftLoopSlope=2.5;
for(uint8 i=29;i>5;i--)
{
if(str->RightEdge[i]!=0)
str->RightEdge[i]=str->RightEdge[i]<(unsigned char)(LeftLoopSlope*i+5)?str->RightEdge[i]:(unsigned char)(LeftLoopSlope*i+5);
else
str->RightEdge[i]=(unsigned char)(LeftLoopSlope*i+5);
if(str->RightEdge[i]>79)
str->RightEdge[i]=0;
}
for(uint8 i=29;i>str->EndLine;i--) //消除左线误搜索
{
if(str->LeftEdge[i]>40)
{
str->LeftEdge[i]=0;
}
if(i<29&&i>ROW_START)
{
if(str->LeftEdge[i]!=0&&str->LeftEdge[i-1]==0&&str->LeftEdge[i+1]==0)
str->LeftEdge[i]=0;
}
}
}
break;
}出环后处理：
在出环后，由于环岛元素存在的缺口，仍需要补线进行引导。也是通过斜率来进行补线。
Round_num=0;
for(Round_row=str->R_StartLine;Round_row>str->EndLine+1&&Round_row>ROW_START+10;Round_row--)
{
if(str->RightEdge[Round_row]==0)
{
Round_num++;
}
}
if(Round_num<3&&((str->R_StartLine-str->EndLine)>22||str->RightEdge[str->EndLine]>18)) //30需要调 huo 判断斜率
{
RoundProcess=Leave_Round2;//暂定
}
else
{
LeftLoopSlope=2.5;
for(uint8 i=29;i>5;i--)
{
if(str->RightEdge[i]!=0)
str->RightEdge[i]=str->RightEdge[i]<(unsigned char)(LeftLoopSlope*i+5)?str->RightEdge[i]:(unsigned char)(LeftLoopSlope*i+5);
else
str->RightEdge[i]=(unsigned char)(LeftLoopSlope*i+5);
if(str->RightEdge[i]>79)
str->RightEdge[i]=0;
}
for(uint8 i=29;i>str->EndLine;i--) //消除左线误搜索
{
if(str->LeftEdge[i]>40)
{
str->LeftEdge[i]=0;
}
if(i<29&&i>ROW_START)
{
if(str->LeftEdge[i]!=0&&str->LeftEdge[i-1]==0&&str->LeftEdge[i+1]==0)
str->LeftEdge[i]=0;
}
}
}之后当车行过缺口就可以恢复正常巡线，这里可以积路程，简单一点，也可以采用起始行是否在图像最底端来判断。
最后
底下附上完整过程代码，以便大家推导理解，对于代码，不建议直接拿去用，因为图像与摄像头参数都不一样。可以根据上面提到的思路，自己写一遍。分阶段来实现进环，至少这套方法我在圆环这里基本没出现过问题，除了速度快了，麦轮没办法很及时的转弯进去。
多嘴一句，这里的方法也不是最好的，但之于我是很有效的。可能也有更快，更好，更丝滑的进环算法。我就充当一个抛砖引玉的角色，希望能帮助没有思路的车友有一种简单有效的方法去实现进环。这一章也是自己用手画的图，在我看来在调试元素的时候，趴赛道才是最好的方法。需要各位仔细琢磨，不断完善，才可以有适合自己的算法。
void Round_Process(Str_Boundary *str,Str_Round *round,Str_Inflection_point *point)
{
float LeftLoopSlope;
float RightLoopSlope;
uint8 break_point_num;
uint8 many_bp_row;
uint8 white_point_num;
uint8 Round_num=0;
break_point_num=0;
switch(RoundType)
{
case Left_Round: //左环
{
switch(RoundProcess)
{
case Find_Round://发现圆环
{
// mark_garage();
// lcd_showint8(110,4, 0);
for(uint8 i=29;i>=0&&i>Boundary.EndLine;i--)
{
if( Boundary.LeftEdge[i]==0 && Boundary.RightEdge[i] )
{
for(uint8 j=Boundary.RightEdge[i];j-1>5;j--)
{
if(mark_middle_num(image[i][j],image[i][j-1],Threshold)
&& abs( difference_sum(image[i][j],image[i][j-1]) )>=g_threshold_value-10 )//跳变
{
break_point_num++;
if(break_point_num>7 )
{
many_bp_row++;
break;
}
}
}
}
if(many_bp_row>3)//黑白跳变点数 90
{
round->Round_flag=0;
RoadType= else_road;
return;
}
}
// lcd_showint8(120,5,many_bp_row);
//*进入下个阶段的判定*/
if(str->L_StartLine<25) //da待改进
{
Round_num=0;
for(Round_row=str->L_StartLine;Round_row>=str->L_StartLine-4&&Round_row>=ROW_START+1;Round_row--) //防止误判 检测换中心岛
{
if(str->LeftEdge[Round_row]!=0)
{
Round_num+=str->LeftEdge[Round_row]-str->LeftEdge[Round_row+1] ;
}
else
{
Round_num=0;
}
}
if(Round_num>10)
{
Round_num=0;
RoundProcess=Find_Gap;//就认为有缺口
// lcd_showint8(80,1, 1);
}
}
//*入环的补线*/
for(Round_row=ROW_END;Round_row>=ROW_START;Round_row--)
{
if(str->RightEdge[Round_row])
{
char temp_r;
char temp_l;
char track;
temp_r=str->RightEdge[Round_row];
track=Boundary.track_Width[Round_row];
temp_l=temp_r-track-1;
if(temp_l>=79)
{
temp_l=78;
}
if(temp_l<=1)
{
temp_l=1;
}
str->LeftEdge[Round_row]=temp_l;
// str->LeftEdge[Round_row]=str->RightEdge[Round_row]-str->track_Width[Round_row];//左边界等于右边界减半宽
}
else
{
str->LeftEdge[Round_row]=0;
}
}
break;
}
case Find_Gap://发现圆环中心
{
// lcd_showint8(110,6, 1);
/*下一阶段的判定*/
//底部三行左边界都不丢线
// if(str->LeftEdge[ROW_END-2]&&str->LeftEdge[ROW_END-1]&&str->LeftEdge[ROW_END-3])//&&str->LeftEdge[ROW_END-2]
// if(Inflection_point.L_up_point.x&&Inflection_point.L_down_point.x&&(Inflection_point.L_down_point.x-Inflection_point.L_up_point.x)<15)
// if(str->LeftEdge[ROW_END-2]&&str->LeftEdge[ROW_END-1]&&str->LeftEdge[ROW_END-3]&&Inflection_point.L_up_point.x)
// mark_garage();
// if(str->LeftEdge[ROW_END-1]-str->LeftEdge[ROW_END-2]>0&&str->LeftEdge[ROW_END-2]-str->LeftEdge[ROW_END-3]>0)
// for(uint8 i=29;i>=0&&i>Boundary.EndLine;i--)
// {
//
//
if( Boundary.LeftEdge[i]==0 && Boundary.RightEdge[i] )
{
// for(uint8 j=Boundary.RightEdge[i];j-1>COL_END-10;j++)
// {
// if(mark_middle_num(image[i][j],image[i][j-1],Threshold)
// && abs( difference_sum(image[i][j],image[i][j-1]) )>=g_threshold_value-10 )//跳变
// {
// break_point_num++;
// if(break_point_num>7 )
// {
// many_bp_row++;
// break;
// }
// }
// }
}
//
// if(many_bp_row>2)//黑白跳变点数 90
// {
// round->Round_flag=0;
// RoadType= else_road;
// return;
// }
// }
if(e_distance>1000)
{
if(Inflection_point.L_up_point.x)
{
RoundProcess=Into_Round;
}
}
/*左补线*/
for(Round_row=ROW_END;Round_row>=ROW_START;Round_row--)
{
if(str->RightEdge[Round_row])
{
// str->LeftEdge[Round_row]=str->RightEdge[Round_row]-str->track_Width[Round_row];//左边界等于右边界减半宽
char temp_r;
char temp_l;
char track;
temp_r=str->RightEdge[Round_row];
track=Boundary.track_Width[Round_row];
temp_l=temp_r-track;
if(temp_l>=79)
{
temp_l=78;
}
if(temp_l<=1)
{
temp_l=1;
}
str->LeftEdge[Round_row]=temp_l;
}
else
{
str->LeftEdge[Round_row]=0;
}
}
break;
}
case Into_Round://进环 关键 分两步
{
// lcd_showint8(110,6, 2);
/*进环的补线处理*/
if(!Inflection_point.L_down_point.x&&!Inflection_point.L_up_point.x&&str->LeftEdge[ROW_END-5])//&&str->LeftEdge[ROW-4])
// if(Inflection_point.L_up_point.x==0&&Inflection_point.L_down_point.x&&str->LeftEdge[ROW_END-3])
// if(Inflection_point.L_up_point.x&&(left_ad>=220||right_ad>200)&&!str->LeftEdge[ROW_END-1])
{
RoundProcess=Into_Round2;
}
else
{ //获取补线点信息
if(point->L_down_point.x>5) //记录上一次拐点存在的信息
{
Inflection_Temp_x=point->L_down_point.x;
Inflection_Temp_y=point->L_down_point.y;
}
//得到信息后，开始补线
for(uint8 i=Inflection_Temp_x;i>2;i--)
{
if(image[i][Inflection_Temp_y]>=Threshold&&image[i-1][Inflection_Temp_y]<Threshold)
{
LeftLoopTemp_R=i-1;
str->EndLine=i-1;
break;
}
}
for(uint8 i=Inflection_Temp_y;i<75;i++)
{
if((image[LeftLoopTemp_R-1][i+1]>=Threshold)&&(image[LeftLoopTemp_R-1][i]<Threshold))
{
LeftLoopTemp_C=i;
break;
}
}
/*进环斜率*/
LeftLoopSlope=1.3;//*(str->RightEdge[aa1]-aa2)/(aa1-LeftLoopTemp_R);
for(uint8 i=0;i<=28;i++)
{
str->RightEdge[LeftLoopTemp_R+i]=(int)(i*LeftLoopSlope+LeftLoopTemp_C);
str->RightEdge[LeftLoopTemp_R+i]=int8_range_protect(str->RightEdge[LeftLoopTemp_R+i],1,79) ;
if(str->RightEdge[LeftLoopTemp_R+i]>78||LeftLoopTemp_R+i>ROW_END)
break;
}
}
for(uint8 i=str->EndLine;i<ROW_END;i++)
{
for(uint8 j=1;j<=79;j++)
{
if(image[i][j]>=Threshold&&image[i][j+1]<Threshold)
{
if(str->RightEdge[i]>j+1)
str->RightEdge[i]=j+1;
break;
}
}
}
break;
}
case Into_Round2://进环 关键 4
{
// lcd_showint8(110,6, 3);
/*判断是否进入环岛*/
if(angle_z<=-40)//&&str->L_StartLine==0&& RoundSize==Small_Round) //定距离一段
{
RoundProcess=In_Round;
Inflection_Temp_x=0;
Inflection_Temp_y=0;
//lcd_showint8(80,5, 4);
}
#if 1 //补线
for(uint8 i=str->EndLine;i<=ROW_END;i++)
{
for(uint8 j=1;j<=75;j++)
{
if(image[i][j]>=Threshold&&image[i][j+1]<Threshold&&image[i][j+2]<Threshold&&image[i][j+3]<Threshold)
//if (difference_sum(image[i][j],image[i][j-COL_SPACE])>=g_threshold_value)
{
if(str->RightEdge[i]>j+1)
str->RightEdge[i]=j+1;
break;
}
}
}
/*进环斜率*/
LeftLoopSlope=2.0;
for(uint8 i=ROW_END;i>ROW_START;i--)
{
// lcd_showint8(80,5, 5);
if(str->RightEdge[i]!=0)
str->RightEdge[i]=str->RightEdge[i]<(int)(LeftLoopSlope*i+5)?str->RightEdge[i]:(int)(LeftLoopSlope*i+5);
else
str->RightEdge[i]=(int)(LeftLoopSlope*i+5);
if(str->RightEdge[i]>79)
str->RightEdge[i]=0;
}
for(uint8 i=29;i>str->EndLine&&i>ROW_START+2;i--)
{
//lcd_showint8(80,5, 6);
if(image[i][10]>=Threshold&&image[i-1][10]<Threshold&&image[i-2][10]<Threshold)
//if (difference_sum(image[i-2][10],image[i][10])>=g_threshold_value)
{
for(uint8 j=i;j>str->EndLine&&j>ROW_START;j--)
{
str->LeftEdge[j]=0;
}
str->EndLine=i-1; //改变endline
break;
}
if(str->LeftEdge[i]>30)
{
str->LeftEdge[i]=0; //限幅
}
}
#endif
break;
}
case In_Round://在环内 5
{
// lcd_showint8(110,4,4);
//记录上一次拐点存在的信息
if(point->R_down_point.x&&image[point->R_down_point.x][point->R_down_point.y-2]>=Threshold&&image[point->R_down_point.x][point->R_down_point.y-3]>=Threshold&&image[point->R_down_point.x-1][point->R_down_point.y]>=Threshold&&image[point->R_down_point.x-2][point->R_down_point.y]>=Threshold)
{
Inflection_Temp_x=point->R_down_point.x;
Inflection_Temp_y =point->R_down_point.y;
//判断下一阶段
if(point->R_down_point.x>5)
{
uint8 hang_num=0; //清除白块计数值 8
for(Round_row=point->R_down_point.x-1; Round_row>=point->R_down_point.x-6&&Round_row>ROW_START ; Round_row-- ) //在搜索到的黑白跳变点后，向上搜索白点，判断是否为干扰点
{
if(str->RowLose[Round_row]==4)
{
hang_num++;
}
}
if( hang_num>2|| point->R_down_point.x>20||Inflection_point.R_down_point.y>30)
{
RoundProcess=Out_Round;
// lcd_showint8(110,4, 4);
}
}
}
if(Inflection_Temp_x>0)
{
//补线
LeftLoopSlope=2.0;
for(uint8 i=Inflection_Temp_x+1;i>ROW_START;i--)
{
str->RightEdge[i]=(int)((i-Inflection_Temp_x)*LeftLoopSlope+Inflection_Temp_y); //fuhao
if (str->RightEdge[i]<COL_START)
{
str->EndLine =i;
break;
}
}
//str->StartLine=ROW_END;
//memset(str->LeftEdge, 0,sizeof(str->LeftEdge) ); //z左线清空
}
for(uint8 i=29;i>str->EndLine;i--) //消除左线误搜索
{
if(str->LeftEdge[i]>30)
{
str->LeftEdge[i]=0;
}
}
break;
}
case Out_Round://出环 6
{
// BlueRing;
// lcd_showint8(110,4, 5);
LeftLoopSlope=2.1;
for(uint8 i=29;i>5;i--)//取小值
{
if(str->RightEdge[i]!=0)
str->RightEdge[i]=str->RightEdge[i]<(unsigned char)(LeftLoopSlope*i+7)?str->RightEdge[i]:(unsigned char)(LeftLoopSlope*i+7);
else
str->RightEdge[i]=(unsigned char)(LeftLoopSlope*i+7);
if(str->RightEdge[i]>79)
str->RightEdge[i]=0;
}
// str->StartLine=ROW_END;
// memset(str->LeftEdge, 0,sizeof(str->LeftEdge) );
//
if(angle_z<-85) //下一阶段标志
{
RoundProcess=Leave_Round;
}
for(uint8 i=29;i>str->EndLine;i--) //消除左线误搜索
{
if(str->LeftEdge[i]>35)
{
str->LeftEdge[i]=0;
}
if(i<29&&i>ROW_START)
{
if(str->LeftEdge[i]!=0&&str->LeftEdge[i-1]==0&&str->LeftEdge[i+1]==0)
str->LeftEdge[i]=0;
}
}
/*下一阶段的判定*/
break;
}
case Leave_Round://离开圆环
{
// lcd_showint8(110,4, 6);
Round_num=0;
for(Round_row=str->R_StartLine;Round_row>str->EndLine+1&&Round_row>ROW_START+10;Round_row--)
{
if(str->RightEdge[Round_row]==0)
{
Round_num++;
}
}
if(Round_num<3&&((str->R_StartLine-str->EndLine)>22||str->RightEdge[str->EndLine]>18)) //30需要调 huo 判断斜率
{
RoundProcess=Leave_Round2;//暂定
}
else
{
LeftLoopSlope=2.5;
for(uint8 i=29;i>5;i--)
{
if(str->RightEdge[i]!=0)
str->RightEdge[i]=str->RightEdge[i]<(unsigned char)(LeftLoopSlope*i+5)?str->RightEdge[i]:(unsigned char)(LeftLoopSlope*i+5);
else
str->RightEdge[i]=(unsigned char)(LeftLoopSlope*i+5);
if(str->RightEdge[i]>79)
str->RightEdge[i]=0;
}
for(uint8 i=29;i>str->EndLine;i--) //消除左线误搜索
{
if(str->LeftEdge[i]>40)
{
str->LeftEdge[i]=0;
}
if(i<29&&i>ROW_START)
{
if(str->LeftEdge[i]!=0&&str->LeftEdge[i-1]==0&&str->LeftEdge[i+1]==0)
str->LeftEdge[i]=0;
}
}
}
break;
}
case Leave_Round2://离开圆环 7
{
//可以拉长视野
// lcd_showint8(110,4, 7);
for(Round_row=ROW_END;Round_row>=ROW_START;Round_row--)
{
if(str->RightEdge[Round_row])
{
// str->LeftEdge[Round_row]=str->RightEdge[Round_row]-str->track_Width[Round_row];//左边界等于右边界减半宽
//if(str->RightEdge[Round_row])
char temp_r;
char temp_l;
char track;
temp_r=str->RightEdge[Round_row];
track=str->track_Width[Round_row]+2.8;
temp_l=temp_r-track;
if(temp_l>=79)
{
temp_l=78;
}
if(temp_l<=1)
{
temp_l=1;
}
str->LeftEdge[Round_row]=temp_l;
}
else
{
str->LeftEdge[Round_row]=0;
}
}
if (Lost_num<2)
{
if(str->L_StartLine&&str->R_StartLine&&str->L_StartLine==29&&str->R_StartLine==29&&Inflection_point.L_down_point.x==0&&Inflection_point.L_up_point.x==0)
{
// lcd_showint8(110,4, 7);
if(str->LeftEdge[ROW_END-2]-str->LeftEdge[ROW_END-1]>0&&str->LeftEdge[ROW_END-4]-str->LeftEdge[ROW_END-3]>0)
{
RoundProcess=Round_Not_Found;//暂定
round->Round_flag=0;
RoadType= else_road;
// BlueQuiet;
}
}
}
break;
}
default:
{
break;
}
}
break;
}
}
评论（0）
您还未登录，请登录后发表或查看评论