路何求
一个小萌新

启程：智能循迹小车

2020年1月12日 4.28k次阅读 共16826个字  
0条评论 12人点赞 路何求
文章目录[x]
  1. 1:1.序言
  2. 2:2.开发环境
  3. 3:3.硬件基础（小车搭建）
  4. 4:4.上层软件
  5. 4.1:4.1基础模块
  6. 4.2:4.2 红外传感器模块
  7. 4.3:4.3 循迹控制
  8. 5:5.总结

目录

1. 序言

2.开发环境

3. 硬件基础

4. 上层软件

     4.1 基础模块

     4.2 红外传感器模块

          4.2.1 TCRT5000

          4.2.2 AMT1450

      4.3循迹控制

5.总结

1.序言

其实这只是一个简单的记录，因为当时——没有拍照……没有拍照……没有拍照。所以就很遗憾。所以打算明年浙江省机器人创新大赛一定要把开发制作过程全部记录下来。

以下正文……

说起玩STM32，其实以前是没有什么想法的，初高中的时候听说过开发板、单片机，当时也萌生过玩一玩的念头，不过初中的时候沉迷于写作，游戏编程，高中的时候由于学业繁忙，所以一直没有开始。

到了大学，加入了机器人创新协会，第一次接触到了单片机，可惜除了第一节课（介绍了一下协会的概况）来了，后面的几节课都因为有事情不好意思地咕咕咕了。只是粗略地了解了一下后面两节课的内容，简单的介绍了一下STM32、环境的安装等等。当时还是有点担心自己会不会落后???（虽然问了去的同学，他们说上不上其实差不多。）

问了会长最近的比赛是浙江省机器人创新大赛，年底会有一个十分水的校内小比赛。

那么就把这个作为短期计划吧。

接下来会介绍利用红外感应的循迹功能的实现（虽然我知道dalao们对此表示这种教程没什么好写的）

2.开发环境

主要开发语言:C语言

1.MDK（KEIL）

2.stm32型号所对应的Pack

3.相应的驱动（JLInk、ST-LINK等所需的驱动程序）

一般卖家都会提供这些东西，也会有一些模板空白工程什么的，倒不用太担心。

Pack包的话如果预置的没有，找卖家要吧，实在没有去keli官网上下载。

3.硬件基础（小车搭建）

一开始是和组员搭建小车，把STM32开发板，小车底座，电机，轮胎，传感器组装在一起。

 

1 stm32开发板*1
2 双轮智能小车底板
3 电机驱动模块*2
4 很垃圾的万向轮*2
5 TCRT5000*2
6 杜邦线*N
7 电池组*1

上面这张图，是我第一次使用的零件，其中红外传感器我用的是两个TCRT5000（  应该是这个型号吧，我也忘了，有点久远了），因为当时第一次接触，基本上啥也不会，全靠百度Google一条龙服务。而且当时主要还是为了基本功能的实现，更主要的是——成功的让小车跑起来。

相应的接线方式，就得根据所购硬件的参考文档来确定了。

tips：有想要自己尝试的可以在淘宝上购买成套的开发板。

4.上层软件

为完成烧录代码的需要，MDK需要进行相应的设置（主要是Target页和Debug页），在菜单栏上方可以看到一个长得像魔法棒一样的东西，点进去就可以看到了。

4.1基础模块

为了让小车跑起来，我们很容易可以想到，我们首先需要让电机动起来。于是，需要让单片机控制对应的IO口（连接了电机），然后再控制电机的转动。这里通过查阅百度，便可以发现PWM控制的概念，也叫“脉冲宽度调制”。

下面对PWM调速作一个蒟蒻的解释:

众所周知,电信号脉冲有高电平和低电平,一个变化周期内,高电平时间长,那么平均输出电压高,控制电机的转速也就越快。

当然，实际情况比这个复杂，而且PWM技术还有很多种控制方法，想了解更多的可以利用好互联网。

（当时，整了好久的这个，准备用定时器设置双路可调PWM的电机程序，当时又是第一次搞这玩意，也基本全靠网上零零散散的教程。后来发现，工程里有已经写好了的MotorDriver和MotorControl，还封装了SetSpeed函数，我直接调用就可以了。）

下面建议不用看，反正也不是我写的，直接跳到4.2部分叭。

//举个串口使能的例子，

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);    //时钟使能
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;         //相应的pin口，这里是PD8
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;     //推免输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //设置输出的速度		
GPIO_Init(GPIOD, &GPIO_InitStructure);            //配置传递函数，跟着改动GPIOX，其中X对应相应的口

上面哪个设置GIOP_Mode写法要看定义的头文件，可能不同，我之前用的其他写法，结果提示未定义。

设置占空比调速的例子：

void MotorDriver_SetPWMDuty(uint8_t nMotor, uint16_t nDuty)
{
    uint16_t nDutySet;
    if(nDuty>PWM_DUTY_LIMIT)nDutySet = PWM_DUTY_LIMIT; //防止超限
    else nDutySet = nDuty;
    switch (nMotor)
    {
    case 1:
        TIM1->CCR4 = nDutySet;
        break;
    case 2:
        TIM1->CCR3 = nDutySet;
        break;
    default:
    ;
    }
}

void MotorDriver_Start(uint8_t nMotor, uint16_t nDuty)
{
    //和上一个函数一样，就是在TIM1-CCRX = nDutySet后面加一个
    //GPIO_ResetBits(GPIOD,GPIO_Pin_8);    //设置为PD8为低电平
}

初始化完电机后，可以写一个方便调用的SetSpeed函数，也有利于后期的数学计算，例如：

void MotorController_SetSpeed(uint8_t nMotor, int16_t nSpeed) 
{
   switch(nMotor)
   {
   case 1:
        MotorController_MotorA_SpeedSet = nSpeed;
        if(MotorDriver_GetMotorState(1) == 1)  
	{
	    MotorController_MotorA_SpeedPWM = PWM_DUTY_LIMIT/2;    //PWM_DuTY_LIMIT是占空比上限值，#Define
	    MotorDriver_Start(1,MotorController_MotorD_SpeedPWM);
	}
	break;
   case 2:
	MotorController_MotorB_SpeedSet = nSpeed;
	if(MotorDriver_GetMotorState(2) == 1)  
	{
	    MotorController_MotorB_SpeedPWM = PWM_DUTY_LIMIT/2;
	    MotorDriver_Start(2,MotorController_MotorD_SpeedPWM);
	}		
	break;
   default:
	;
   }
}

4.2 红外传感器模块

4.2.1 TCRT5000红外传感器

使用传感器嘛，流程就是看参考的技术文档，然后接线，最后写代码。

接线就是根据需要，我第一次使用的TCRT5000用的是PB15和PE5，看下图红箭头，至于问我当初为什么选这两个。

原因有二：1.当时啥都不懂。2.这样接比较对称。

 

写代码嘛就是根据技术文档调用函数（如果有）。

针对这里的TCRT5000，因为这个传感器很简单，所以没有。

TCRT发射红外线，经过反射，激发接收器，返回高电平或者低电平。（比如：反射面为黑色区块，红外线被吸收一部分，反射的强度不足以达到激发阈值，返回低电平，传感器上的LED指示灯亮。）

于是，我们很容易可以想到，我们就需要设定阈值，也就是校准。

将小车放在地上，把传感器置于黑色区块上方，用螺丝刀调节校准螺帽，直到确保黑色区块上方刚好灯亮，白色区块上方刚好灯灭。

硬件上调整好了，那么下一步就是写代码了。

于是，我们想到，步骤是1.初始化串口。2.读取串口数据。3.编写便于调用的封装函数。

代码如下：

TCRT5000.h

#ifndef __TCRT5000_H    //宏定义：条件编译
#define __TCRT5000_H

#include "stm32f4xx.h"      //头文件包含

#define TCRT5000_1 GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_5)     //读取PE5数据
#define TCRT5000_2 GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_15)

void TCRT5000_config(void);                          //初始化函数

#endif

TCRT5000.c

#include "TCRT5000.h"

void TCRT5000_config(void)
{
GPIO_InitTypeDef GPIO_InitStructure;    //定义结构体
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);    //时钟使能

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15；             //第15管脚
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;       //输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;                //第5管脚
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;     //输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOE, &GPIO_InitStructure);                        //设置IO口初始化
}

我们可以看到，我们已经可以用TCRT5000_1和TCRT5000_2读取红外传感器返回的数据，于是我们可以开始写最最简单的循迹功能了。

循迹地图如下:(红色为小车启动区)

 

代码如下：

extern int Map_Stop;   //停止计数
extern int history;          //历史循迹数据暂存

SystemInit();
TimingDelay_Decrement();  //延时初始化
TCRT5000_config();              //红外传感器设置
map_load();                              //加载地图参数
while(1)
{
//这里的死循环不用担心，因为STM32用的Cortex内核有异常相应系统，包括中断事件，可以写一个中断控制按键。

//根据接线判断，右侧为TCRT5000_1,左侧为TCRT5000_2。
//TCRT5000_n==0表示未被激发（探测到黑色区块）TCRT5000_n==1表示被激发（白色区块）。具体那个是激发需先测试。
if(TCRT5000_1==0&&TCRT5000_2==0)        //均未激发,白线居中,直行
{
MotorController_SetSpeed(1,-200);   //继续向前
MotorController_SetSpeed(2,200);
Delay_ms(10);
}
if(TCRT5000_1==1&&TCRT5000_2==0)//右侧被激发,白线在右,车身左偏
{
MotorController_SetSpeed(1,-230);   //车身左偏,小车左轮加速,右轮减速－＞小右转
MotorController_SetSpeed(2,170);
Delay_ms(10);
}
if(TCRT5000_1==0&&TCRT5000_2==1)//左侧被激发,白线在左,车身右偏
{
MotorController_SetSpeed(1,-170);    //车身右偏,小车右轮加速,左轮减速－＞小左转
MotorController_SetSpeed(2,230);
Delay_ms(10);
}

//history==0表示上一次探测不是十字路口
if(history==0&&TCRT5000_1==1&&TCRT5000_2==1)　//均被激发,探测到横向白线,进入十字路口。
{
history=1;       //标记为进入十字路口
map_read();  //读取地图数据并判断，执行直角转弯、直行、停车等
Delay_ms(10);
}
if(Map_Stop==1){}

4.2.2 AMT1450红外传感器

当然,我们很容易可以发现上面那个很简单的循迹功能是很简陋的,而且鲁棒性很差，很容易在转弯/校正航线的时候过转。因为它的探测是非线性的，只有小车前左和前右两个采样点，如果白线居中还好，但万一出现校正方向过转，白线到两个采样点一侧，就会失效。

如果，采取增大采样点距离的措施，就会导致灵敏性下降，偏一点的时候探测不到。如果速度过快，会导致校正过晚冲出白线，或者走之字形折线前进。

（其实我本来想剪个这些制作过程的视频，也更好体现。但是还是那句话，当时没考虑到，没有足够的摄像记录留下，剪辑素材不够。）

鉴于以上缺点，于是采用了另一个传感器AMT1450。

通过查看文档可以看到，该传感器最大可以提供144点采样数据，可以粗略算是一种线性采样。

该传感器可以用uart串口，且在amt1450_uart.c中看到，初始化串口通信时用了uart5，于是将其连接到uart5。

amt1450_uart.c的一部分（反正不是我写的，收起来）

本质上还是初始化串口的一条龙流程。定义结构体，使能时钟，连接，设定pin，设定模式等等。

void init_AMT1450_UART(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure; //NVIC中断向量结构体
/* config UART5 clock */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);

/* Connect UART5 pins to AF */
GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_UART5);
GPIO_PinAFConfig(GPIOD, GPIO_PinSource2, GPIO_AF_UART5);

/* USART5 GPIO config */
/* Configure UART5 Tx (PC.12) as alternate function push-pull */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_PuPd =GPIO_PuPd_UP;
GPIO_Init(GPIOC, &GPIO_InitStructure);
/* Configure UART5 Rx (PD.02) as input floating */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_PuPd =GPIO_PuPd_UP;
GPIO_Init(GPIOD, &GPIO_InitStructure);

/* Enable the UART5 OverSampling by 8 */
USART_OverSampling8Cmd(UART5, ENABLE);

/* UART5 mode config */
USART_InitStructure.USART_BaudRate = 115200;
// USART_InitStructure.USART_BaudRate = 19200;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No ;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(UART5, &USART_InitStructure);

/* Enable UART5 Receive and Transmit interrupts */
USART_ITConfig(UART5, USART_IT_RXNE, ENABLE);
USART_ITConfig(UART5, USART_IT_TXE, ENABLE);
// 优先级配置
NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);

// USART_Cmd(UART5, ENABLE);
AMT1450_UART_Cmd(DISABLE);
}

上面只是amt1450_uart.c源文件的以部门，下面还有关于读取传感器返回的数据的函数。

//获取amt1450数据
void get_AMT1450Data_UART(uint8_t *begin_Color,uint8_t *jump_Count,uint8_t *jump_Location)
{
    *begin_Color = (amt1450_UART_Rx.ValidData[1] & 0x80) >>7;
    *jump_Count = amt1450_UART_Rx.ValidData[1] & 0x0f;
    for (uint8_t i=0;i<*jump_Count;i++)
    {
        jump_Location[i] = amt1450_UART_Rx.ValidData[2+i];
    }
}

查看文档可知：

uint8_t *begin_Color存储最左端采样点的值，如：1表示白，0表示黑。

uint8_t *jump_Count存储跳变次数，“线性”采样数据中，一次颜色变化计为一次跳变。

uint8_t *jump_Location存储跳变的位置

get_AMT1450Data_UART(&begin, &jump, count);            //讲数据存储在三个变量中
if(jump == 2) position = 0.5f * (count[0] + count[1]); //position=两次跳变的中间位置

使用的是工作模式是144点采样数据，跳变位置在0~144之间，如果跳变次数为两次，即如下图所示：

（因为没有照片，所以就顺手建了个很菜鸡的模型，有点丑，别介意，能看就好 ）

 

黄色的矩体代表前面那张图里长长的AMT1450，其正下方为“线性”的探测区域。

返回信号从白色到黑色再到白色，跳变两次，jump变量为2。然后position=0.5f * (count[0] + count[1])，即计算两个跳变点（黑线的两个边缘）的中间位置。

…… 突然意识到……地图上应该是是黑色背景，白色循迹线。我建模的时候似乎搞反了，算了，反正这图就是示意一下。

4.3 循迹控制

那么有了数据，就可以用来写循迹函数了。

上文提到，计算的position可以作为黑线的位置，假定目标值为72，即处于正中间。当产生偏差时，如position为100、43等等，就可以体现小车左偏或右偏，然后据此进行两轮调速。

我们肯定会希望：

1.偏差越大，校正力度越大，偏差越小，只作微调。

2.偏差变化越快（偏离方向速度越快），校正力度越大。

于是，很自然就会引出传统的PID控制办法。（具体的原理和优缺点可以自行再做研究，这里不再赘述。）

通过一个PID函数，将position值进行处理（减去期望值），得到一个误差（输入量），基于这个误差对小车的速度进行控制，导致小车位置变化，又得到一个新的position，即得到一个新的误差，再一次把这个新误差作为下一轮的输入量。如此往复……

即基于误差（当前的误差，过去的误差，误差的变化趋势）来消除误差。

//***************************************************
//数据结构&参数设定 *
//***************************************************

int trackSpeed;    //循迹速度

typedef struct Track_PID   //定义PID参数结构体变量
{
int target; //设定目标
long error_acc; //误差累计值

double track_KP; //比例常数
double track_KI; //积分常数
double track_KD; //微分常数

int lastError;
int prevError;
}Track_PID;

static Track_PID sPID;
static Track_PID *PIDpoint = &sPID;

extern uint8_t begin,jump,count[6]; // *最大六次跳变
extern uint8_t position;

//历史探测数据暂存
int8_t trackHistory; //1:黑 0:白
//十字路口计数
int8_t nCrossing;

//****************************************************
//循迹初始化 *
//****************************************************
void TrackInit(void)
{
trackSpeed=HIGH_STD_SPEED;   //在头文件中定义
trackHistory=0;
nCrossing=0;
PIDInit();      //初始化PID参数
Map_load();
MotorController_SetSpeed(1,-trackSpeed);
MotorController_SetSpeed(2, trackSpeed);
}

//*************PID参数初始化**************************
void PIDInit(void)
{
PIDpoint->error_acc =0 ;
PIDpoint->lastError =0 ;
PIDpoint->prevError =0 ;

PIDpoint->track_KP =2.6 ;
PIDpoint->track_KI =0 ;        //这里没有使用积分，所以值赋为零
PIDpoint->track_KD =1.4 ;
PIDpoint->target =76 ;
}
//****************************************************
//位置式P(I)D计算子函数 *
//****************************************************

int TrackControl_PID(int input)
{
int iError,output=0;
iError = PIDpoint->target - input; //误差值计算
PIDpoint->error_acc += iError; //积分
Output = PIDpoint->track_KP * iError
+ PIDpoint->track_KI * PIDpoint->error_acc*0.5f
+ PIDpoint->track_KD * iError-PIDpoint->lastError;
//error值存储
PIDpoint->lastError = iError;
return(output);
}

//****************************************************
//循迹子函数*
//****************************************************

void TrackControl(void)
{
while(1)
{
get_AMT1450Data_UART(&begin, &jump, count);
if(jump == 2) position = 0.5f * (count[0] + count[1]);
MotorController_SetSpeed(1,-trackSpeed+TrackControl_PID(position));  //使用TrackControl_PID()函数，对position进行处理。
MotorController_SetSpeed(2, trackSpeed+TrackControl_PID(position));
if(CrossingJudging()==1)break;  //当判定路口为真时跳出循环。
Delay_ms(10);
}

}

//*****************************************************
//十字路口判定 *
//*****************************************************

int CrossingJudging(void)
{
int8_t out;
out=0;
if(trackHistory==1 && begin==0 && jump==0)  //trackHistory==1表示上一次检测不是十字路口，最左边（begin）为白色，且零次跳变，说明探测到横向白线
{
trackHistory=0;  //赋为零，表示进入十字路口
nCrossing+=1;     //路口计数+1
out=1;
}
if(trackHistory==0 && begin==1)
{
trackHistory=1;
}
return(out);
}

当然，在实际操作中还会遇到很多问题。

比如，有时候会发现车身突然抖动，特别是在经过十字路口的时候更为明显，因此，我们需要增加一个过滤到那些突变值的函数。

我使用了一阶低通滤波，并且把它合并到了PID控制函数的末尾。

一阶低通滤波

首先，在PID结构体变量中增加两个变量

float filterPercent; //一阶低通滤波系数(0,1]
int lastFilter; //前次滤波值

 

然后，在PID参数初始化中增加两个初始化语句

PIDpoint->filterPercent =0.3;
PIDpoint->lastFilter =0;

 

最后在PID控制子函数的末尾，对output进行处理

//输出值滤波
output = PIDpoint->filterPercent * output + (1 - PIDpoint->filterPercent)* PIDpoint->lastFilter;
PIDpoint->lastFilter = output;//滤波值存储

 

其他不在赘述。

当然，仅仅能够沿着一条线去行驶，还是远远不够的。我们需要直角转弯，倒车，加速，减速等等。

我在制作过程中，对什么时候转弯进行了多次迭代，下面取其中两个具有代表性的版本。

第一代转弯方案：十字路口全探测方案

什么时候转？

前文以及提到了nCrossing十字路口计数，当到达指定路口时，执行转弯函数即可。

怎么转？

一轮停止，另一轮转动。

什么时候转好？

当探测器再一次判断到begin为白色时。

 

为了实现这三个功能，方法时极其简单的。

只需要对nCrossing进行一下if的逻辑判断即可。

注意：下面这个使用TCRT5000做的，使用AMT1450时一样的道理。我没有留下使用AMT1450的版本。而且，这里又三个TCRT5000，TCRT5000_0是在中间增加了一个。

int n=10;  //上限值

void map_load(void)
{
//----------------------------------------
// 载入地图
//----------------------------------------
//map[n]=? 0停止，1直行，2左转，3右转，4掉头。
//对map[]数组进行处理
}

void map_read(void)
{
//-----------------------------------------
// 读取地图
//-----------------------------------------
nCrossing+=1;
switch(map[nCrossing])
{
case 1 :
MotorController_SetSpeed(1,-200);
MotorController_SetSpeed(2,200);
Delay_ms(100);
while(1)
{
if(TCRT5000_0==0&&(TCRT5000_1==1||TCRT5000_2==1))break;
Delay_ms(10);
}
break;

case 2 :
MotorController_SetSpeed(1,0);               //一轮停止，一轮转动
MotorController_SetSpeed(2,380);
Delay_ms(100);
while(1)
{
if(TCRT5000_0==0||TCRT5000_1==0)break;    //转弯完成，跳出函数
Delay_ms(10);
}
break;

case 3 :
MotorController_SetSpeed(1,-380);
MotorController_SetSpeed(2,0);
Delay_ms(100);
while(1)
{
if(TCRT5000_0==0||TCRT5000_2==0)break;
Delay_ms(10);
}
break;

case 4 :
break;

default :
MotorController_SetSpeed(1,0);
MotorController_SetSpeed(2,0);
Map_Stop=1;
}
history=0;
}

当然，这些代码还是不完善的，因为是竞速比赛，所以需要更快。但是提高了速度就会导致转弯时稳定性急剧下降，因此我又增加了减速函数和提速函数。

首先在头文件中增加两个常量，分别是（加速）加速度，和（减速）加速度。

在转弯前先使用减速函数和提速函数，便可以做到，直线高速，转弯慢性的策略。

当然，仅仅如此而是远远不够的，我们需要更快，所以就想到了圆弧路径，类似漂移一样的经过转弯口，切入下一条路，那么就可以全程高速不减速。

预览慢动作视频：


（部分手机端如果显示异常，如仅局部，可点击链接：查看视频）

上面这个视频就是方案二的效果视频，可以点击下面右方的小“＋”号展开。

第二代转弯方案：模型引入的判断方案

什么时候转弯？

快要到路口的时候转。

怎么转？

走圆弧路径。

什么时候转好？

转90°时。

 

为了实现这三个功能，自然要想到，我需要如何判断什么时候转。

这也是模型引入的用处之一。

通过对速度积分取得位移，对角速度积分取得角度。

通过预先输入命令形式1.0m，或者转动Π/2，对小车的轮胎等效半径和小车的车身等效直径进行校准，使trackSpeed的值与SI结合。

如让trackSpeed=500刚好表示速度为500mm/s

首先增加两个变量的定义：

int calDistance;                     //计算距离值
int8_t derrorDistance;               //里程计容忍值

增加初始化

calDistance=0;
derrorDistance=0;
//****************************************************
//直行距离控制子函数 *
//****************************************************

void StraightDistanceControl(int straightSpeed,int targetDistance)
{
calDistance=0;
trackSpeed=straightSpeed;
MotorController_SetSpeed(1,-trackSpeed);
MotorController_SetSpeed(2,trackSpeed);
do
{
Delay_ms(10);
calDistance+=trackSpeed*0.01f; //积分距离
get_AMT1450Data_UART(&begin, &jump, count);
if(jump == 2) position = 0.5f * (count[0] + count[1]);
MotorController_SetSpeed(1,-trackSpeed+TrackControl_PID(position));
MotorController_SetSpeed(2, trackSpeed+TrackControl_PID(position));
}while(calDistance<=targetDistance);
}
//****************************************************
//旋转控制子函数 *
//****************************************************

//其中的CROSS_DIF_STD_Speed,MY_CAR_Diameter都是一些宏定义的常量，分别是转弯差速和车身等效直径。

void RotationAngleControl(int iCarRotationWheel,double iCarRotationAngle)
{
double flashAngle;
flashAngle=0;
iCarRotationAngle=iCarRotationAngle*3.14f/180;
if(iCarRotationWheel==1)  //单轮转
{
do
{
Delay_ms(10);
flashAngle+=(1.0*(trackSpeed+CROSS_DIF_STD_Speed))/1000*0.01/(1.0*MY_CAR_Diameter/1000); //角度积分公式
}while(flashAngle<=iCarRotationAngle);
}
else if(iCarRotationWheel==2)  //双轮转
{
do
{
Delay_ms(10);
flashAngle+=(1.0*trackSpeed/1000*0.01f/(1.0*MY_CAR_Diameter/1000*0.5f));
}while(flashAngle<=iCarRotationAngle);
}
}

当然这些要实现快速过弯还是不够的，上面俩个只是简单的直行和转弯，校准，掉头，转弯提前距离判断用的都是这两个函数。

下面是向左的快速过弯函数，分别是90°小弯和180°大弯。向右转弯同理可得。

其中，形如MY_CAR_Diameter，CORNER_Radius，SMALL_ANGLE_DIS这样的变量全都是宏定义常量，容易放在同一个被包含的头文件中，用于调试时直接修改。

//*****************************************************
//转弯函数 *
//*****************************************************

void CarLeft(void)
{
double flashAngle;
StraightDistanceControl(trackSpeed,SMALL_ANGLE_DIS);
MotorController_SetSpeed(1,-(CORNER_Radius-MY_CAR_Diameter/2)/CORNER_Radius*trackSpeed);
MotorController_SetSpeed(2,(CORNER_Radius+MY_CAR_Diameter/2)/CORNER_Radius*trackSpeed);
flashAngle=0;
do
{
Delay_ms(10);
flashAngle+=(1.0*(trackSpeed))/1000*0.01/(1.0*CORNER_Radius/1000);
}while(flashAngle<=90*3.14/180);
nCrossing+=1;
}

void BigCarLeft(void)
{
double flashAngle;
StraightDistanceControl(trackSpeed,BIG_ANGLE_DIS);
MotorController_SetSpeed(1,-(BIG_Radius-MY_CAR_Diameter/2)/BIG_Radius*trackSpeed);
MotorController_SetSpeed(2,(BIG_Radius+MY_CAR_Diameter/2)/BIG_Radius*trackSpeed);
flashAngle=0;
do
{
Delay_ms(10);
flashAngle+=(1.0*(trackSpeed))/1000*0.01/(1.0*BigRadius/1000);
}while(flashAngle<=180*3.14/180);
nCrossing+=2;
}

5.总结

应该没什么漏了的把……漏了我也不管了……

emmm好像少了个地图载入和地图读取函数。算了固定路线，反正就是一个数组的事情，不写了。

当然，最后的结果自然也是比较喜人的。比赛一共是按照预定轨迹跑三圈，正跑一圈，逆跑一圈，再正跑一圈抵达终点。

总用时37.98 s，平均一圈12.66 s，下图是悄咪咪拍下的最开始比的几组。

奖品是一个树莓派……

好吧，一个以前听闻但没玩过的东西，似乎又要多折腾一样东西了233……

本作品采用 知识共享署名-相同方式共享 4.0 国际许可协议 进行许可
点赞
最后编辑：2020年11月2日

发表评论

昵称和uid可以选填一个，邮箱必填(用于评论被回复时的邮件提醒，不会被公开)
tips:输入uid可以获取你的B站昵称和头像