stm32篇--小车循迹

小车的循迹算法

循迹小车使用红外对射寻黑线，根据轨道的不同，我使用了两种不同算法：

(1)第一种是在轨道外有影响的，比如轨道外有黑色，车出去一点就会碰到黑色，那就是检测到歪了之后除非检测它

回来了，否则一直拐弯：

//自己寻道

void Self_Tracting(void)

{

        while(1)

 {

          test_sensor();

  motor_move_speed(paras.speed);

              if(GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_3))//传感器5

             {

            GPIO_ResetBits(GPIOF,GPIO_Pin_11); //点亮LED5           

            //左边界，减速

      motor_move_steer(paras.right_angle);//车右

      

            while(!GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_4));

     GPIO_ResetBits(GPIOB,GPIO_Pin_0); //熄灭LED3

     motor_move_steer(150);//车直行,140 for20120417,before is 150   

        }

        else

            GPIO_SetBits(GPIOF,GPIO_Pin_11); //熄灭LED5

        if(GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_2)) //传感器4

        {

            GPIO_ResetBits(GPIOB,GPIO_Pin_1); //点亮LED4           

      motor_move_steer(paras.right_angle);//车右

     

      while(!GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_4));

      motor_move_steer(150);//车直行,140 for20120417,before is 150   

        }else

            GPIO_SetBits(GPIOB,GPIO_Pin_1); //熄灭LED4

     

        if(GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_4))//传感器3

        {

            GPIO_ResetBits(GPIOB,GPIO_Pin_0); //点亮LED3

             motor_move_steer(150);//车直行,140 for20120417,before is 150

        }else

            GPIO_SetBits(GPIOB,GPIO_Pin_0); //熄灭LED3

     

        if(GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_5))//传感器2

        {

            GPIO_ResetBits(GPIOC,GPIO_Pin_5); //点亮LED2          

       motor_move_steer(paras.left_angle);//车左转

      

       while(!GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_4));

      motor_move_steer(150);//车直行,140 for20120417,before is 150    

        }else

            GPIO_SetBits(GPIOC,GPIO_Pin_5); //熄灭LED2

        if(GPIO_ReadInputDataBit(GPIOF , GPIO_Pin_6))//传感器1

        {

            GPIO_ResetBits(GPIOC,GPIO_Pin_4); //点亮LED1

              //左边界，减速

    motor_move_steer(paras.left_angle);//车左转

     

      while(!GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_4)); 

      motor_move_steer(150);//车直行,140 for20120417,before is 150

        }else

            GPIO_SetBits(GPIOC,GPIO_Pin_4); //熄灭LED1

         

  if(GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_15))

   break;  

 }

}

(2)第二种是为适应车道宽且车道外没有黑色斑迹的，可能同时亮两个灯但是不会是检测出错

//自己寻道

void Self_Tracting(void)

{

 unsigned char left_angle_1,left_angle_2;

 unsigned char right_angle_1,right_angle_2;

 unsigned char tpw;

 left_angle_2 = (paras.left_angle + paras.angle)/2;

 left_angle_1 = (paras.left_angle + left_angle_2)/2;

 right_angle_2 = (paras.right_angle + paras.angle)/2;

 right_angle_1 = (paras.right_angle + right_angle_2)/2;

 motor_move_speed(paras.speed);

 

 while(1)

 {

  uart_print("The car begin1!!",16);

  test_sensor();

        tpw = 0x00;

  

  /* 传感器检测 */

  if(GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_3))

        {

   tpw |= 0x01;

   GPIO_ResetBits(GPIOF,GPIO_Pin_11); //点亮LED5 

  }

  else

   GPIO_SetBits(GPIOF,GPIO_Pin_11);  //熄灭LED5

   

  if(GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_2))

  {

   tpw |= 0x02;

   GPIO_ResetBits(GPIOB,GPIO_Pin_1);  //点亮LED4 

  }

  else

   GPIO_SetBits(GPIOB,GPIO_Pin_1);  //熄灭LED4

  

  if(GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_4))

  {

   tpw |= 0x04;

   GPIO_ResetBits(GPIOB,GPIO_Pin_0);  //点亮LED3 

  }

  else

   GPIO_SetBits(GPIOB,GPIO_Pin_0);  //熄灭LED3

  

  if(GPIO_ReadInputDataBit(GPIOE , GPIO_Pin_5))

  {

   tpw |= 0x08;

   GPIO_ResetBits(GPIOC,GPIO_Pin_5);  //点亮LED2 

  }

  else

   GPIO_SetBits(GPIOC,GPIO_Pin_5);  //熄灭LED2

  

  if(GPIO_ReadInputDataBit(GPIOF , GPIO_Pin_6))

  {

   tpw |= 0x10;

   GPIO_ResetBits(GPIOC,GPIO_Pin_4);  //点亮LED1 

  }

  else

   GPIO_SetBits(GPIOC,GPIO_Pin_4);  //熄灭LED1

  

  /* 动作执行 */

 switch(tpw)

  {

   case 0x07:

    motor_move_steer(paras.right_angle+2); //右转弯

    motor_move_speed(paras.speed+20);

    break;

   case 0x01: case 0x03:

    motor_move_steer(paras.right_angle+2); //右转弯

    motor_move_speed(paras.speed+15);

    break;

   case 0x02:

    motor_move_steer(right_angle_2);  //微量右转弯

    motor_move_speed(paras.speed+5);

    break;

   case 0x06:

    motor_move_steer(right_angle_1);  //微量右转弯

    motor_move_speed(paras.speed);

    break;

   case 0x04:

    motor_move_steer(paras.angle);   //车直行

    motor_move_speed(paras.speed-5);

    break;

   case 0x0c:

    motor_move_steer(left_angle_1);   //微量左转弯

    motor_move_speed(paras.speed);

    break;

   case 0x08:

    motor_move_steer(left_angle_2);   //微量左转弯

    motor_move_speed(paras.speed+5);

    break;

   case 0x10: case 0x18:

    motor_move_steer(paras.left_angle-2);  //左转弯

    motor_move_speed(paras.speed+15);

    break;

   case 0x1c:

    motor_move_steer(paras.left_angle-2);  //左转弯

    motor_move_speed(paras.speed+20);

    break;

   default:

    break;

  }

  if(GPIO_ReadInputDataBit(GPIOB , GPIO_Pin_15))

   break;

 }

(3)还有一个算法，没太研就其中的PID算法，听同事说就是随便给两个量

带进去公式之后就可以缓慢的更新数值知道和预定的数值相等.