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