路径识别代码
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
路径识别代码
识别是用来设定速度的。。。简单的识别还是容易，贴个代码。。。
其中gap值为极左极右差值，附
for(i=49;i>=(M_Row_End+3);i--) // recored_end_row
{
if(Black_Flag[i].Mid_flag&&Black_Flag[i-3].Mid_flag)
{
n = Runway_Midpoint[i-3] - Runway_Midpoint[i];
if(n>=2)
S_right++;
else if(n<=-2)
S_left++;
elseS_straight++;
}
}
void Road_S_Dis(void)
{
prespeed_value = speed_value;
if(curValidline<11)
{
if(gap<=24) //包括了直道和小S弯
{
straight_flag++;
straight_flag%=3000;
}
else if(gap<=65) //入弯口
{
if(S_right>3&&S_left>3) Big_S = 1;
elsebend_flag=1;
}
else {zhongsu++;zhongsu%=3000;}//较远前瞻的弯，给定中速
zhongsu3_flag = 0;
danwan_flag = 0;
}
else if(M_Row_End<=22)
{
if(gap>=75||S_right>= (50-M_Row_End)-6||S_left>=50-M_Row_End - 6)
{danwan_flag++;danwan_flag%=3000;} //单向弯道，给定中速2
else if(gap>=45)
{
if(S_right>3&&S_left>3) jiman_flag=1;//较近前瞻的S弯，给定慢速
else {zhongsu2_flag++;zhongsu2_flag%=3000;} //较近前瞻的缓慢程度弯道，给定中速
danwan_flag = 0;
}
else { jiaokuaisu_flag=1;danwan_flag = 0; } //较近前瞻的平缓曲线，给定较快速度
zhongsu3_flag = 0;
}
else if(M_Row_End<=33)
{
if(gap>=80) { danwan2_flag++;danwan2_flag%=3000;}//近前瞻的单向弯道，给定中速
else if(gap>=50||S_right> 50-M_Row_End-5||S_left>50-M_Row_End-5) //近前瞻的急偏道，给定中慢速
{
zhongsu3_flag++;zhongsu3_flag%=3000;
}
else { mansu_flag=1;zhongsu3_flag = 0 ; }//弯入十字中慢速
danwan_flag = 0;
}
else
{
if(gap>=20) {mansu++;mansu%=3000;}
else { kuaisu++;kuaisu%=3000; } //直道入十字情况
zhongsu3_flag = 0;
danwan_flag = 0;
}
if(straight_flag==1) Hightest_speed = longtest_speed;
else if (bend_flag==1) Hightest_speed = longtest_speed - 75 ;
else if (Big_S==1) Hightest_speed = longtest_speed - 80 ;
else if (zhongsu == 1) Hightest_speed = MidLongtest_speed + 10;
else if (danwan_flag> 0)
{
if(danwan_flag == 1)
Hightest_speed = MidLongtest_speed;
else if(danwan_flag == 2)
Hightest_speed = MidLongtest_speed + 50;
elseHightest_speed = Hightest_speed+10>longtest_speed+50?longtest_speed+50:Hightest_speed + 10;
}
else if (jiman_flag == 1) Hightest_speed = Midtest_speed;
else if (zhongsu2_flag == 1) Hightest_speed = MidLongtest_speed - 20;
else if (jiaokuaisu_flag == 1) Hightest_speed = MidLongtest_speed + 20;
else if (danwan2_flag == 1) Hightest_speed = MidLongtest_speed ;
else if (zhongsu3_flag>0) //进入十字弯减速，然后加速
{ if(zhongsu3_flag==1)
Hightest_speed = MidLongtest_speed;