本次队内赛我的寻迹分为三个阶段从一开始的慢速寻迹到2.0版本的可以快速过直角弯再到3.0版本减少摆头的寻迹。
目录
一.初版寻迹
开始我使用的是8路寻迹,但是由于是淘的寻迹模块,所以需要一个一个调,但是效果不理想所以换了一个之前捡的五路寻迹的模块,就此展开寻迹之路。
首先要初始化寻迹的每个引脚,其次要对每个引脚的电平进行读取,最后再把电平的高低逻辑与转弯逻辑相结合。
下面是代码部分:
1.初始化寻迹的每个引脚
void Track_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD ,ENABLE);
//IO管脚模式配置为浮空输入,这样就能获取传感器传回来的数字信号(高低电平)
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_11|GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOD,&GPIO_InitStructure);
}
我是用的是宏定义放在了track.h里面对每个引脚的电平进行读取
#ifndef __TRACK_H
#define __TRACK_H
#include "sys.h"
#define LED1 GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_11)
#define LED2 GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_12)
#define LED3 GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_13)
#define LED4 GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_14)
#define LED5 GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_15)
void Track(void);
void Track_Init(void);
#endif
初代逻辑就不展示了,他可以寻迹,但是需要非常慢的速度,不然直角弯会过不去,之后我开始想如何不以牺牲速度的情况下来过直角弯。
二.直角弯逻辑
首先想到不能快速过直角弯是因为速度会太快冲出去,之后灯会全亮,那我在if(左转/右转)嵌套一个if(灯全灭)就左转/右转,可不可以呢?实践告诉:逻辑很丰满,现实很骨感,通过现象发现在if(左转/右转)的前提下下一次灯亮灭的逻辑大概率不是灯全灭(因为距离下一次灯全亮,这中间会穿插一些其他的灯亮灭的逻辑),所以就会从逻辑中跳出来进入不了左右转。
之后我引入了历史电平的逻辑成功解决了这种问题,思路就是在灯全亮之后检测上一次的历史电平在作出相应的转向
//*检测到 黑线灯灭输出 0*/
int led1,led2,led3,led4,led5;//记录上一次电平
if(LED1==1&&LED3==0&&LED5==1)
{
led1=LED1;led2=LED2;led3=LED3;led4=LED4;led5=LED5;
GoAhead();
}
//左转00011||00111||01111||10011||10111
else if((LED5==1&&LED4==1)&&((LED1==0&&LED2==0&&LED3==0)||(LED1==0&&LED2==0&&LED3==1)||(LED1==0&&LED2==1&&LED3==1)||(LED1==1&&LED2==0&&LED3==0)))
{
led1=LED1;led2=LED2;led3=LED3;led4=LED4;led5=LED5;
LeftS pin();
}
//右转11100||11110||11001||11000||11101
else if((LED1==1&&LED2==1)&&((LED3==1&&LED4==0&&LED5==0)||(LED3==1&&LED4==1&&LED5==0)||(LED3==0&&LED4==0&&LED5==1)||(LED3==1&&LED4==0&&LED5==1)))
{
led1=LED1;led2=LED2;led3=LED3;led4=LED4;led5=LED5;
RightSpin();
}
else if(LED1==1&&LED2==1&&LED3==1&&LED4==1&&LED5==1)//冲出去之后判断上一次的电平做出判断
{
if((led5==0)||(led4==0)||(led4==0&&led5==0)||(led3==0&&led4==0&&led5==0)||(led2==0&&led3==0&&led4==0&&led5==0)||(LED3==0&&LED4==0))//1 2 3 4 5
{
RightSpin();//1 2 3 4 5
}
else if((led1==0)||(led2==0)||(led1==0&&led2==0)||(led1==0&&led2==0&&led3==0)||(led1==0&&led2==0&&led3==0&&led4==0)||(LED2==0&&LED3==0))
{
LeftSpin();
}
else if(led1==0&&led2==0&&led3==0&&led4==0&&led5==0)//停止位
{
Stop();
}
}
部分代码如上所示。并且在每一次的if里面都取到上一次的历史电平
代码基本完成但是我发现摆头次数太多也会影响寻迹的速度开始尝试减少摆头的次数
三.减摆头逻辑
当时首先想到的是一种比较极端的逻辑,减少摆头那我就让他每次都冲出去再进行旋转。
这里我画图来解释:
如果按照之前传统的逻辑那么就是A->B->C->D这种3灯灭->4灯灭就旋转那么这种弯道能转好几次但是如果能实现只有最后一个灯灭那么再转(或者可以说是每次冲出去之后再转向)就是图中: E->F->G->H这种旋转方式那么摆头次数就少很多。
那么所有转弯的逻辑我都写在if(灯全灭)嵌套if(检测历史电平)再作出转向。并且我给转向一个条件然他转到特定的方向再停止。
代码如下
if(LED1==1&&LED3==0&&LED5==1)
{
led1=LED1;led2=LED2;led3=LED3;led4=LED4;led5=LED5;
GoAhead();
}
//左转00011||00111||01111||10011||10111
else if((LED5==1&&LED4==1)&&((LED1==0&&LED2==0&&LED3==0)||(LED1==0&&LED2==0&&LED3==1)||(LED1==0&&LED2==1&&LED3==1)||(LED1==1&&LED2==0&&LED3==0)))
{
led1=LED1;led2=LED2;led3=LED3;led4=LED4;led5=LED5;
// while((LED3==0)!=1)
// {
// LeftS pin();
// }
}
//右转11100||11110||11001||11000||11101
else if((LED1==1&&LED2==1)&&((LED3==1&&LED4==0&&LED5==0)||(LED3==1&&LED4==1&&LED5==0)||(LED3==0&&LED4==0&&LED5==1)||(LED3==1&&LED4==0&&LED5==1)))
{
led1=LED1;led2=LED2;led3=LED3;led4=LED4;led5=LED5;
// while((LED3==0)!=1)
// {
// RightSpin();
// }
}
else if(LED1==1&&LED2==1&&LED3==1&&LED4==1&&LED5==1)//冲出去之后判断上一次的电平做出判断
{
if((led5==0)||(led4==0)||(led4==0&&led5==0)||(led3==0&&led4==0&&led5==0)||(led2==0&&led3==0&&led4==0&&led5==0)||(LED3==0&&LED4==0))//1 2 3 4 5
{
while(((LED5==1&&LED3==0))!=1)
{
RightSpin();//1 2 3 4 5
}
}
else if((led1==0)||(led2==0)||(led1==0&&led2==0)||(led1==0&&led2==0&&led3==0)||(led1==0&&led2==0&&led3==0&&led4==0)||(LED2==0&&LED3==0))
{
while(((LED3==0&&LED1==1))!=1)
{
LeftSpin();
}
}
else if(led1==0&&led2==0&&led3==0&&led4==0&&led5==0)
{
Stop();
}
}
注:我把前面的转向全部注释掉,只做记录历史电平作用,所有操作仅在从出去之后进行!
代码思路到此基本结束,但是要做到稳定需要再根据实际情况进行调整
后来我根据场地又适当增添了一些转弯的逻辑增加寻迹的稳定性。