简单的巡线

部分代码如下:这个代码是在ros下做的:

  strcpy(a,light_status_.c_str());// light_status_.c_str()是arduino采集来的IR的状态

  for (i=0;i<8;i++)

  {

    if (a[i]=='D')

    {

      k++;

      b[i]=1;

    }

    ROS_INFO("b[%d]=%f",i,b[i]);

  }

  ROS_INFO("k=%d",k);

  switch (k)

  {

    case 0:

    case 8:

                angle=0.0;

                switch_state=0;

                break;

    case 1:

    case 2:   

    case 3:

    case 8:

               for (j=0;j<8;j++)

                 angle+=(b[j]*(3.5-j)*0.1578);

                 ROS_INFO("angle-1:%f",angle);

                 angle=angle/k;

                 switch_state=1;

               break;

    case 4:

    case 5:

    case 6:

    case 7:

                for (j=0;j<8;j++)

                 angle+=(b[j]*(3.5-j)*0.1578);

                 ROS_INFO("angle4:%f",angle);

                 angle=(angle*10/k);

                 switch_state=1;

                break;

  }

将这个angle传给PID运算器,进行pid控制,pid控制器的返回值,将其发送给速度话题控制偏转.