c32循迹小车c语言程序,stm32篇--小车循迹

素颜马尾好姑娘i 2022-10-14 05:46 214阅读 0赞

小车的循迹算法

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

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