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算法，听同事说就是随便给两个量
带进去公式之后就可以缓慢的更新数值知道和预定的数值相等.