十三届智能汽车竞赛信标对抗组思路参考程序参考（四）寻灯策略Part.②

最新推荐文章于 2023-04-08 23:37:03 发布

ZZM丶

最新推荐文章于 2023-04-08 23:37:03 发布

阅读量2k

点赞数 7

文章标签：嵌入式单片机

本文链接：https://blog.csdn.net/qq_41833526/article/details/109557767

版权

系列链接：

正文开始：
上一篇分享了小车在看见一个信标灯亮起时如何寻找一个较优的寻灯和避灯路线，这次打算接着上一篇继续分享寻灯策略的问题。

信标比赛的整个流程是：场地点亮一个信标灯，靠小车接近把信标灯灭掉，场地再点亮随机一个信标灯，循环往复，直至小车灭掉所有信标灯，计总时间。意味着小车会交替面临画面中有灯和无灯的两种情况，这就需要小车能够在看不见信标灯的时候主动环视场地，寻找亮起的信标灯。看不见灯的时候，小车以最大打角进行原地旋转，直至看见信标灯：

if(x_MID==0)                                              //没看见灯
{
  NewPWMValue=99;                                                   //我的前轮舵机在PWM占空比570±99时即可达到最大打角
}
FTM_PWM_Duty(FTM2,CH1,570+NewPWMValue);                           //我的前轮舵机在PWM占空比为57%时打正，因此在570基础上加减PD输出

此时无灯状态也解决了，搭配上一篇分享的有灯状态的方案，小车已经具备了完成整场信标比赛的能力了，但实验的时候发现还是会有一些问题：比如在原地旋转找灯的时候会撞上刚刚灭掉的灯，向亮起的信标灯跑去的时候会撞到中间没亮的信标灯，等情况，影响总计时，甚至小车被卡主无法完成任务。接下来接分享我们队当时逐一解决这些问题的想法和方案，供大家参考。
为了方便说明，先控制一个变量：过灯侧始终为右侧，即小车从信标灯的右侧避过并灭灯。如下图：橙色实心圆为当前亮起的灯，黑色虚线空心圆为信标灯的灭灯范围，蓝色线条代表小车运行路线，可以小车灭灯后看不见灯时原地旋转的方向来区分，大体分为两种方案，第一种即灭灯后以刚灭掉的灯的反方向旋转，第二种即为以刚灭掉的灯的同方向旋转。
下图假设某一种情况，绿色实心圆为第二个亮起的灯，方位在过灯侧的相反侧。可见这两种旋转找灯的方案路线就出现了巨大的差别，显然这种情况下第二种旋转方向所行驶的路程更短。
下图，再添加第三个亮起的灯为黄色实心圆，在过灯侧的同侧。此时第一种旋转方案显然优于第二种，而且第二种方案小车会撞上信标灯。这种情况是碰巧的，的确，第一种方案也会在某种情况下撞击刚灭掉的信标灯，那就只能通过比较两种方案的撞击概率来判断谁比较好一些了。
如下图，把两种旋转的路线画在同一张图上，可以很明显的看出两个圆与刚灭掉的信标灯的距离是不同的，可以分析造成这种现象的原因：小车从侧方离开信标灯即将进入旋转找灯的状态时，画一条虚线段来表示小车行驶方向，即图中绿色虚线，两种旋转方案即可看成是以绿色虚线为对称轴画两个直径相同的圆，且与绿色虚线相切，而绿色虚线并不过信标灯圆心，因此与信标灯在同一侧的旋转圆更靠近信标灯。
为了不干扰观看，把小车、灭灯路线去掉。下图，作黑色直线为按小车旋转圆行驶方向与信标灯两侧的切线，切线之间的夹角即可认为是小车撞灯的范围，即如果下一个信标灯存在于红色双箭头标识的夹角内，小车就会撞击这个橙色信标灯。可以证明，旋转圆离灯较近的第二种方案其夹角更大，撞击信标灯的可能情况更多。综上，我们队选择了第一种方案，即灭灯后以刚灭掉的灯的反方向旋转，以此降低撞击刚灭掉的灯的概率。

if(x_MID==0)                                              //没看见灯
{
  NewPWMValue= -99;                                                   //我的前轮舵机在PWM占空比570±99时即可达到最大打角
}
FTM_PWM_Duty(FTM2,CH1,570+NewPWMValue);                           //我的前轮舵机在PWM占空比为57%时打正，因此在570基础上加减PD输出

结合两篇寻灯策略，放上完整的寻灯舵机控制函数

void duoji_PWM()
{  
  if(x_MID>0)   //看见有灯
    {
      jieshouwu=0;
      SHANSHUO=0;
      PWM_P=y_MID/4-4;
      suduyiY=27.385-Speed_test*6/65;
  //    suduyiX=8.615-Speed_test*7/65;
      suduyiX=6.615-Speed_test*6.5/65;
      if((x_MID<40)&&(panK==0))
      {
        panK=1;
        k=1;
      }
      else if((x_MID>=40)&&(panK==0))
      {
        panK=1;
        k=-1;
      }

      if(y_MID<38) NewPWMValue=(int)((x_MID-40)*2);
      else if(y_MID<57) NewPWMValue=(int)((x_MID-((-0.12)*pow((y_MID-suduyiY),2)*k + 1.814*(y_MID-suduyiY)*k + 40-suduyiX*k))*PWM_P);
      else NewPWMValue=k*99;
      
      if((NewPWMValue>0)&&(PTE12_INT==1)) NewPWMValue=-80;
      if((NewPWMValue<0)&&(PTE11_INT==1)) NewPWMValue=80;

      zhangai_PWM(k);
      nicePWM=NewPWMValue+youPWM+zuoPWM+zhongPWM;
    }
  else        //无灯//
    {
        SHANSHUO++;
        if(SHANSHUO>4) 
        {
          zhangai_PWM(k);
          panK=0;
          if(((PTE10_INT==0)||(PTE11_INT==0)||(PTE12_INT==0))&&(jieshouwu==0))      ///看不见但是红外有接收/
          {
            jieshouwu=1;
            if((PTE9_INT==0)||(PTE12_INT==0)) k=-1;
            if((PTE10_INT==0)||(PTE11_INT==0)) k=1;
          }
          if(SHANSHUO<7)  nicePWM=youPWM+zuoPWM+zhongPWM+k*99;
          else            nicePWM=youPWM+zuoPWM+zhongPWM+(k*(Speed_test<<2));  //绕障碍 速度大打角大
        }
    }
  if(nicePWM>99) nicePWM=99;         //打角限幅
  else if(nicePWM<-99) nicePWM=-99;
}