怎样做直道和弯道的判断，是用什么差值法

无亲 · 发表于 2012-4-17 16:14:51

求高手

ab123369ab · 发表于 2012-4-18 15:39:47

识别是用来设定速度的。。。简单的识别还是容易，贴个代码。。。
其中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++;
      else    S_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;
      else bend_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;
  else Hightest_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;
else if(zhongsu3_flag==2)Hightest_speed = MidLongtest_speed+40;
else Hightest_speed = Hightest_speed + 10 > longtest_speed+50?longtest_speed+50:Hightest_speed + 10;
}
else if (mansu_flag == 1) Hightest_speed = MidLongtest_speed - 30;
else if (mansu == 1) Hightest_speed = Midtest_speed + 30;
else if (kuaisu == 1) Hightest_speed = MidLongtest_speed;

   speed_percent = POW2(offset)/2500; if(speed_percent>1) speed_percent=1;
   motercontrl = (Hightest_speed-lowtest_speed)*speed_percent;
   speed_value = Hightest_speed-motercontrl;
   if(L_Row_End == 49&& R_Row_End == 49) speed_value=prespeed_value;
   if(speed_value < 80 ) speed_value = 80;

   Carry_out_speedCOUNT++;
  if(Carry_out_speedCOUNT>30)
  {
   Carry_out_speed(speed_value);
   Carry_out_speedCOUNT = 31;
  }
}