STM32机器人

2020-03-01 18:41:29 来源：范文大全收藏下载本文

上海第二工业大学

实训报告

课程名称:一级项目（机器人）_ 学生姓名：周勇学号： 20124823566 学院名称：电子与电气工程学院专业班级： 12自动化C1

第1章

绪论

1.1 研究背景和意义

机器人是一个很好的科研平台，非常适合以它作为工程对象，来学习和掌握软件编程、嵌入式技术、传感器技术、无线数据通信、机电一体化、图像处理与模式识别及人工智能等专业知识。机器人已经广泛地应用于工业、医学、农业、建筑业、以及军事等领域，本项目通过用STM32单片机制作一个简单机器人，实现一些机器人必备的基本动作来进一步学习STM32单片机，提高我的实践操作能力，以弥补课堂上单纯的理论知识所造成的欠缺。此外，参与机器人的制作会将理论知识与实践完美地结合在一起，制作的产品会给我们带来一种喜悦与自豪之感，这将进一步激发我们的学习热情，引领我们走向更远的专业之路。

1.2国内外机器人发展方向

随着机器人在社会中的不断普及和应用，人们对机器人实用化、市场化的呼声也越来越高。未来机器人的发展将有一个非常大的发展空间。未来机器人可能会朝着一下几个方面发展：

（1）外形结构的拟人化和微型化

机器人发展的初级阶段是以机械手的形式呈现的。当时由于技术、资金、性价比等方面的原因，并没有注重发展拟人机器人。随着技术的不断进步，还有机械手式机器人呈现的不足，拟人化的机器人正在得到快速的发展。微型化也是机器人发展的一个目标，研制纳米级的机器人可以进入人体内部，帮助人体清理垃圾，治疗疾病等。（2）功能的多样化

机器人以后的发展方向必然是向着多功能、多扩展性的方向发展，听、视、触、嗅、味觉将会是机器人必备的功能。在现今的机器人中，听、视觉发展相对较快，在触、嗅、味觉方面的由于技术上的限制，现在还没有研制出在这些方面比较成熟的感应装置。

（3）具有自我学习和思考的能力

机器人相对于人最大的不足就是不具有思考和再学习的能力，这也是由于多方面原因造成的。首先人类对自身的这种能力的认识也不是很彻底。其次机器人的大脑（CPU）的运算能力还是不如人脑。不过相信在不远的将来，人类对于这种能力的认识足够清楚之后，并且更加强大的CPU将能被研制出来。机器人也将很快就具有这种思考和自我学习的能力了。

第2章系统总体设计思路

2.1 项目设计要求

基于STM32开发板的基础上，通过以I/O口控制伺服电机的转动为起点逐步增加操作的复杂程度，最终使得小车可以实现复杂运动、智能探索和避开障碍物、距离检测并显示在液晶屏上以及进行小车尾随等功能。

2.2 硬件系统设计方案

本项目所使用的智能小车为欧鹏科技有限公司生产的，采用STM32F103系类微控制器。小车由两个舵机和一个前方的万向轮来控制其运动。在小车的前方有一块面包板，便于我们自己搭建一些额外的外围电路模块，以增强小车的实用性和智能型。小车使用的是两节3.7V的直流电源给电机供电，另外通过三端电源稳压芯片7805以及C1117芯片将7.4V的直流电源转化为3.3V的直流电源供STM32芯片使用。小车的底面和正面分别见图1-1和1-2以及下载程序时的连接线路图如图1-3所示。

图1-1 小车底面图

图1-2 小车正面图

第3章系统模块软硬件设计 3.1机器人伺服电机控制信号

本项目中所用的机器人伺服电机型号为PARALLAX。电机实物图见图2-1.控制伺服电机转动速度的信号如图2-

2、2-3和2-4所示的脉冲信号。图2-2所示是高电平持续1.5ms 低电平持续20ms，然后不断重复地控制脉冲序列。该脉冲序列发给经过零点标定后的伺服电机，伺服电机不会旋转。如果此时你的电机旋转，表明电机需要标定。此时，我们需要参考参考《基础机器人制作》教材中有关伺服电机标定的部分。从图2-

2、2-3和2-4可知，控制电机运动转速的是高电平持续的时间，当高电平持续时间为1.3ms时，电机顺时针全速旋转，当高电平持续时间1.7ms时，电机逆时针速旋转。下面我们将看到如何给单片机微控制器编程使P1端口的第一脚（P1_0）来发出伺服电机的控制信号。

图2-1

项目中所使用的舵机实物图

图2-2 电机转速为零的控制信号时序图

图2-3 1.3 ms的控制脉冲序列使电机顺时针全速旋转

图2-4 1.7ms的连续脉冲序列使电机逆时针全速旋转

3.1.1 舵机控制

通过P1端口的第一脚（P1_0）来发出伺服电机的控制信号驱动一个舵机。下面的程序代码

1、代码

2、代码3可以分别让一个舵机（舵机的标准状态下）静止不动、沿顺时针方向以最大速度转动、沿逆时针方向以最大速度转动。代码1 while (1)

{

GPIO_SetBits(GPIOC, GPIO_Pin_10);

// PD10输出高电平

delay_nus(1500);

// 延时1500微秒

GPIO_ResetBits(GPIOC,GPIO_Pin_110);

// PD10输出高电平

delay_nms(20000);

//延时20毫秒 }

代码2 while (1)

{

GPIO_SetBits(GPIOD, GPIO_Pin_10);

// PD10输出高电平

delay_nus(1300);

// 延时1300微秒

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

// PD10输出高电平

delay_nms(20);

//延时20毫秒

}

代码3 while (1)

{

GPIO_SetBits(GPIOD, GPIO_Pin_10);

// PD10输出高电平

delay_nus(1700);

// 延时1700微秒

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

// PD10输出高电平

delay_nms(20);

//延时20毫秒

}

3.1.2 驱动小车

以上的三段程序代码可以让我们来控制一个舵机的正反转以及停止这三种状态。下面这段代码4是控制两个鸵机同时沿顺时针方向全速转动的程序，这样我们便可以让个小车向前跑起来。

代码4 #include "stm32f10x_heads.h" #include "HelloRobot.h" int main(void) {

BSP_Init();

USART_Configuration();

printf("Program Running!\n");

while (1)

{

GPIO_SetBits(GPIOD, GPIO_Pin_10);

// PD10输出高电平delay_nus(1300);

//延时1300微秒

GPIO_ResetBits(GPIOD, GPIO_Pin_10)；

// PD10输出高电平

GPIO_SetBits(GPIOD,GPIO_Pin_9);

// PD10输出高电平

delay_nus(1700);

//延时1700微秒 GPIO_ResetBits(GPIOD,GPIO_Pin_9);

// PD10输出高电平

delay_nms(20);

//延时20毫秒

} }

3.2 循环语句控制小车的运行时间长短

到目前为止，已经明白了脉冲宽度控制连续旋转电机速度和方向的原理。控制电机速度和方向的方法是非常简单的。控制电机运行的时间也非常简单，那就是用 for 循环。下面代码5是 for 循环的例子，它会使小车向前运动大约3秒钟。

代码5 #include "stm32f10x_heads.h" #include "HelloRobot.h" int main(void) { int counter; BSP_Init(); USART_Configuration(); printf("Program Running!\n");

for(counter=0;counter

GPIO_SetBits(GPIOD, GPIO_Pin_9); delay_nus(1300); GPIO_ResetBits(GPIOD,GPIO_Pin_9); delay_nms(20);

} while(1); }让我们来计算一下这个代码能使电机转动的确切的时间长度。每通过循环一次delay_nus(1700)持续1.7 ms，delay_nms(20)持续20ms，再delay_nus(1300)，其他语句的执行时间很少，可忽略。

那么for循环整体执行一次的时间是：1.7 ms + 20 ms+1.3 ms = 23ms，本循环执行130次，即就是23ms乘以130，时间=130*23ms =130*0.023秒=3秒。这样我们的这个电机就可以沿逆时针方向转动大约3秒的时间了。这样就可以有效控制小车运动时间的长短。

3.3 用C语言函数进行机器人巡航控制

前面我们已经知道控制电机前进、后退以及左右转等简单操作，下面我们通过建立数组来让小车进行复杂的连续性的运动。要点：建立一个数组，通过switch、case语句来让小车执行相应的命令，最终完成小车的复杂运动。相应的程序如下代码6所示：

代码6 #include "stm32f10x_heads.h" #include "HelloRobot.h" void Forward(void)

//建立一个小车前进的子函数 {

int i;

for(i=1;i

//延时大约1.5秒

{

GPIO_SetBits(GPIOD, GPIO_Pin_10);

delay_nus(1700);

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

GPIO_SetBits(GPIOD, GPIO_Pin_9);

delay_nus(1300);

GPIO_ResetBits(GPIOD,GPIO_Pin_9);

delay_nms(20); } } void Left_Turn(void)

//建立一个小车左转的子函数 {

int i;

for(i=1;i

//延时大约0.65秒

{

GPIO_SetBits(GPIOD, GPIO_Pin_10);

delay_nus(1300);

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

GPIO_SetBits(GPIOD, GPIO_Pin_9);

delay_nus(1300);

GPIO_ResetBits(GPIOD,GPIO_Pin_9);

delay_nms(20); } } void Right_Turn(void)

//建立一个小车右转的子函数 {

int i;

for(i=1;i

//延时大约0.65秒

{

GPIO_SetBits(GPIOD, GPIO_Pin_10);

delay_nus(1700);

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

GPIO_SetBits(GPIOD, GPIO_Pin_9);

delay_nus(1700);

GPIO_ResetBits(GPIOD,GPIO_Pin_9);

delay_nms(20); } } void Backward(void)

//建立一个小车后退的子函数 {

int i;

for(i=1;i

//延时大约1.5秒

{

GPIO_SetBits(GPIOD, GPIO_Pin_10);

delay_nus(1300);

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

GPIO_SetBits(GPIOD, GPIO_Pin_9);

delay_nus(1700);

GPIO_ResetBits(GPIOD,GPIO_Pin_9);

delay_nms(20); } } int main(void) {

char Navigation[10]={'F','L','F','F','R','B','L','B','B','Q'};

int addre=0;

BSP_Init();

USART_Configuration();

printf("Program Running!\n");

while(Navigation[addre]!='Q')

{

switch(Navigation[addre])

{

case 'F':Forward();break;

case 'L':Left_Turn();break;

case 'R':Right_Turn();break;

case 'B':Backward();break;

}

addre++; } while(1); }

3.3.1代码分析

在程序主函数中定义了一个字符数组如下所示： char Navigation[10]={'F','L','F','F','R','B','L','B','B','Q'};

这个数组中存储的是一些命令：F表示向前运动，L表示向左转，R表示向右转，B表示向后退，Q表示程序结束。之后，定义了一个int型变量addre，用来作为访问数组的索引。接着是一个while循环，注意到这个循环的条件表达式与前面的不同：只有当前访问的数组值不为Q时，才执行循环体内的语句。在循环内，每次执行switch语句后，都要更新addre，以使下次循环时执行新的运动。 switch语句

switch语句是一种多分支选择语句，其一般形式如下：

switch(表达式){

case 常量表达式 1: 语句 1;break; case 常量表达式 2: 语句 2;break; „

case 常量表达式 n: 语句 n;break; default:

语句 n+1;break;

}

其语义是：计算表达式的值，逐个与其后的常量表达式值相比较，当表达式的值与某个常量表达式的值相等时，即执行其后的语句。如表达式的值与所有case后的常量表达式均不相同时，则执行default后的语句。

在本例程中，当Navigation[addre]为’F’时，执行向前运动的函数Forward()；当

Navigation[addre]为’L’时，执行向左转的函数Left_Turn()；当Navigation[addre]为’R’时，执行向右转的函数Right_Turn()；当Navigation[addre]为’B’时，执行向后运动的函数Backward()。

3.4 单片机输入接口与机器人触觉导航

许多自动化机械都依赖于各种触觉型开关，例如当机器人碰到障碍物时，接触开关就会察觉，通过编程让机器人躲开障碍物；旅客登机桥在靠近飞机时为了保护昂贵的飞机，在登机桥接口安装触须，当登机桥离飞机很近后触须就会碰到飞机，立即通知控制器提醒离飞机已经很近了，需要降低靠近速度；工厂利用触觉开关来计量生产线上的工件数量；在工业加工过程中，也被用来排列物体。在所有这些实例中，触觉开关提供的输入通过计算机或者单片机处理后生成其它形式的程序化的输出。 3.4.1触觉导航与单片机输入接口

STM32单片机的5组GPIO端口既可以作为输入，也可以作为输出，既可按16位处理，也可按位方式使用。实际上，当单片机启动或复位时，复用功能未开启，IO端口被配制成浮空输入模式。所有端口都有外部中断能力。为了使用外部中断线，必须将端口配置成输入模式。

作为输入，如果I/O脚上的电压为3.3V，则其相对应的I/O口寄存器中的相应位存储1；如果电压为0V，则存储0。

布置恰当的电路，可以让胡须达到上述效果：当胡须没有被碰到时，使I/O脚上的电压为3.3V；当胡须被碰到时，则使I/O脚上电压为0。然后，单片机就可以读入相应数据，进行分析、处理，控制机器人的运动。 3.4.2安装并测试机器人胡须

机器人的胡须安装实物图如图2-5所示。图2-6是其电路简化图，图2-7是小车触须的完整实物图。

图2-5

机器人胡须

图2-6 机器人胡须电路示意图

图 2-7 机器人的触须实物图

通过编程让单片机探测什么时候胡须被触动。由图2-6可知，连接到每个胡须电路的I/O 引脚监视着10K上拉电阻上的电压变化。当胡须没有被触动，连接胡须的I/O管脚的电压是3.3V；当胡须被触动时，I/O短接到地，所以I/O管脚的电压是0V。

3.4.3 基于胡须的机器人触觉导航

在机器人行走过程中，如果有胡须被触动，那就意味着碰到了什么。导航程序需要接受这些输入信息，判断它的意义，调用一系列使机器人倒退、旋转朝不同方向行走的动作子函数以避开障碍物。

下面的程序让机器人向前走直到碰到障碍物。在这种情况下，机器人用它的一根或者两根胡须探测障碍物。一旦胡须探测到障碍物，调用C语言函数控制机器人巡航导航中的导航程序和子程序使小车倒退或者旋转，然后再重新向前行走，直到遇到另一个障碍物。为了实现这些功能，需要编程机器人来做出选择，此时要用到if语句的另一种形式，if-else-if形式，它可以进行多分支选择。它的一般形式为：

if(表达式1)

语句1;

else if(表达式2)

语句2;

else if(表达式3)

语句3; „„

else if(表达式n-1)

语句n-1; Else 语句n;

其语义为：依次判断表达式的值，当出现某个值为真时，则执行其对应的语句。然后跳到整个if语句之外继续执行程序；如果所有的表达式均为假，则执行语句n。然后继续执行后续程序。

3.4.4机器人进入死区后人工智能决策程序代码7如下:

代码7

#include "stm32f10x_heads.h" #include "HelloRobot.h"

int PE2state(void)

//获取PE2胡须的状态 { return GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_2); } int PE3state(void)

//获取PE3胡须的状态 { return GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_3); }

void Forward(void) {

GPIO_SetBits(GPIOD, GPIO_Pin_10);

delay_nus(1700);

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

GPIO_SetBits(GPIOD, GPIO_Pin_9);

delay_nus(1300);

GPIO_ResetBits(GPIOD,GPIO_Pin_9);

delay_nms(20); } void Left_Turn(void) {

int i;

for(i=1;i

GPIO_SetBits(GPIOD, GPIO_Pin_10);

delay_nus(1300);

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

GPIO_SetBits(GPIOD, GPIO_Pin_9);

delay_nus(1300);

GPIO_ResetBits(GPIOD,GPIO_Pin_9);

delay_nms(20); } } void Right_Turn(void) {

int i;

for(i=1;i

GPIO_SetBits(GPIOD, GPIO_Pin_10);

delay_nus(1700);

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

GPIO_SetBits(GPIOD, GPIO_Pin_9);

delay_nus(1700);

GPIO_ResetBits(GPIOD,GPIO_Pin_9);

delay_nms(20); } } void Backward(void) {

int i;

for(i=1;i

GPIO_SetBits(GPIOD, GPIO_Pin_10);

delay_nus(1300);

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

GPIO_SetBits(GPIOD, GPIO_Pin_9);

delay_nus(1700);

GPIO_ResetBits(GPIOD,GPIO_Pin_9);

delay_nms(20); } } int main(void) {

BSP_Init();

USART_Configuration(); printf("Program Running!\n");

while(1) {

if((PE2state()==0)&&( PE3state()==0))

//两胡须同时碰到

{

Backward();

//向后退

Left_Turn();

//向左转

Left_Turn();

//向左转

}

else if(PE2state()==0)

//右边胡须碰到

{

Backward();

//向后退

Left_Turn();

//向左转

}

else if(PE3state()==0)

//左边胡须碰到

{

Backward();

//后退

Right_Turn();

//向右转

}

else

//胡须没有碰到

Forward();

//前进

} }

3.5接口综合应用与红外导航

红外线二极管发射红外光，如果机器人前面有障碍物，红外线从物体反射回来，相当于机器人眼睛的红外检测（接收）器，检测到反射回的红外光线，并发出信号来表明检测到从物体反射回红外线。机器人的大脑——STM32基于这个传感器的输入控制伺服电机。

红外线（IR）接收/检测器有内置的光滤波器，除了需要检测的980 nm波长的红外线外，它几乎不允许其它光通过。红外检测器还有一个电子滤波器，它只允许大约38.5 kHz 的电信号通过。换句话说，检测器只寻找每秒闪烁38,500次的红外光。这就防止了普通光源象太阳光和室内光对IR的干涉。太阳光是直流干涉（0Hz）源，而室内光依赖于所在区域的主电源，闪烁频率接近100或120 Hz。由于120 Hz在电子滤波器的38.5 kHz通带频率之外，它完全被IR探测器忽略。

3.5.1 搭建电路并测试红外发射和接收.电路如图-所示。

电路中使用三极管的原因是STM32单片机的I\O口电压为3.3V，为了提高其驱动能力，这里加入三极管（工作电压为5V）使其工作在开关状态驱动红外发射管，而不是直接用STM32单片机驱动。

图2-8 红外发射和接收理论电路图

图2- 9红外发射和接收实物图

3.5.2 测试红外发射探测器

下面要用PE1发送持续1毫秒的38.5kHz的红外光，如果红外光被小车路径上的物体反射回来，红外检测器将给微控制器发送一个信号，让它知道已经检测到反射回的红外光。让每个IR LED 探测器组工作的关键是发送1毫秒频率为38.5 kHz的红外信号，然后立刻将IR探测器的输出存储到一个变量中。下面是一个例子，它发送38.5 kHz信号给连接到PE1 的IR发射器，然后用整型变量irDetectLeft存储连接到P1_2的IR探测器的输出。

for(counter=0;counter

{

GPIO_SetBits(GPIOE, GPIO_Pin_0);

delay_nus(13);

GPIO_ResetBits(GPIOE, GPIO_Pin_0);

delay_nus(13);

} 上述代码给P1_3输出的信号高电平13微秒，低电平为13微秒，总周期为26微秒，即频率约为38.5kHz。总共输出38个周期的信号，即持续时间约为1毫秒（38*26约等于1000微秒）。当没有红外信号返回时，探测器的输出状态为高。当它探测到被物体反射的38500Hz红外信号时，它的输出为低。因红外信号发送的持续时间为1毫秒，因此IR探测器的输出如果处于低，其持续状态也不会超过1毫秒，因此发送完信号后必须立即将IR探测器的输出存储到变量中。这些存储的值会显示在调试终端或被机器人用来导航。

3.5.3 误差原因分析 : 当我们用数字示波器测量一下PE1产生的方波的频率并不是严格的38.5kHz，而是比38.5kHz略低。为什么会这样呢？这是因为上面例程中除了延时函数本身严格产生13us的延时外，延时函数的调用过程也会产生延时，因此实际产生的延时会比13微秒更长。函数调用时，CPU会先进行一系列的操作，这些操作是需要时间的，至少几个us，所要求的延时也是微秒级，这就造成了延时的不精确性。

3.5.4 高性能红外导航

有关IR检测器的一个有趣的事是它们的输出与触须的输出非常相象。没有检测到物体时，输出为高；检测到物体时，输出为低。

在触须导航里使用的预编程机动动作很好，但是在使用IR LED和探测器时会造成不必要的迟钝。发送脉冲给电机之前检查障碍物，可以大大改善机器人的行走性能。程序可以使用传感器输入为每个瞬间的导航选择最好的机动动作。这样，机器人永远不会走过头，它会找到绕开障碍物的完美路线，成功的走过更加复杂的路线。 3.5.5 在每个脉冲之间采样以避免碰撞

探测障碍物很重要的一点是在机器人撞到它之前给机器人留有绕开它的空间。如果前方有障碍物，机器人会使用脉冲命令避开，然后探测，如果物体还在，再使用另一个脉冲来避开它。机器人能持续使用电机驱动脉冲和探测，直到它绕开障碍物，然后它会继续发向前行走的脉冲。

其详细程序代码8如下所示:

代码8 #include "stm32f10x_heads.h" #include "HelloRobot.h"

#define LeftLaunch_1 GPIO_SetBits(GPIOE, GPIO_Pin_1)

//左边红外发射 #define LeftLaunch_0 GPIO_ResetBits(GPIOE, GPIO_Pin_1) //左边红外发射 #define RightLaunch_1 GPIO_SetBits(GPIOE, GPIO_Pin_0)

//右边红外发射 #define RightLaunch_0 GPIO_ResetBits(GPIOE, GPIO_Pin_0) //右边红外发射 #define LeftIR

GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_3) //左边红外接收 #define RightIR GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_2) //左边红外接收 void IRLaunch(unsigned char IR) {

int counter;

if(IR=='L') //左边发射

for(counter=0;counter

{

LeftLaunch_1;

delay_nus(12);

LeftLaunch_0;

delay_nus(12);

}

if(IR=='R') //右边发射

for(counter=0;counter

{

RightLaunch_1;

delay_nus(12);

RightLaunch_0;

delay_nus(12);

} }

int main(void) { int pulseLeft,pulseRight; int irDetectLeft,irDetectRight;

BSP_Init();

USART_Configuration();

printf("Program Running!\r\n");

{

IRLaunch('R'); //右边发射

irDetectRight = RightIR;//右边接收

IRLaunch('L'); //左边发射

irDetectLeft = LeftIR;//左边接收

if((irDetectLeft==0)&&(irDetectRight==0))//向后退

{

pulseLeft=1300;

pulseRight=1700;

}

else if((irDetectLeft==0)&&(irDetectRight==1))//向右转

{

pulseLeft=1700;

pulseRight=1700;

}

else if((irDetectLeft==1)&&(irDetectRight==0))//向左转

{

pulseLeft=1300;

pulseRight=1300;

}

Else

//前进

{

pulseLeft=1700;

pulseRight=1300;

}

GPIO_SetBits(GPIOD, GPIO_Pin_10);

delay_nus(pulseLeft);

GPIO_ResetBits(GPIOD,GPIO_Pin_10);

GPIO_SetBits(GPIOD, GPIO_Pin_9);

delay_nus(pulseRight);

GPIO_ResetBits(GPIOD,GPIO_Pin_9);

delay_nms(20);

}

while(1); }

程序解析：

这个程序使用稍微不同的方法来使用驱动脉冲。除了两个存储IR检测器输出的状态以外，它还使用两个整型变量来设置发送的脉冲持续时间。 int pulseLeft,pulseRight;

int irDetectLeft,irDetectRight;

前面，我们使用了当型循环控制语句while，它的一般表达式为：while(表达式) 语句; 这里，我们使用另一种循环控制语句： do„while语句

在C语言中，直到型循环控制语句是“do„while”，它的一般形式为： do 语句while(表达式);

其中，语句通常为复合语句，称为循环体。

do„while语句的基本特点是：先执行后判断。因此，循环体至少被执行一次。在do循环体中，发送38.5 kHz的IR信号给每个IR LED。当脉冲发送完后，变量立即存储IR检测器的输出状态。这是很有必要的，因为如果你等待的时间太长，无论是否发现物体，将返回没有探测到物体的状态1。

3.6 尾随小车

3.6.1尾随概念及其原理

让一个机器人跟随另一个机器人行走，跟随的机器人也叫尾随车。尾随车要正常工作必须知道距离引导车有多远。如果尾随车落在后面，它必须能察觉并加速。如果尾随车距离引导车太近，它也要能察觉并减速。如果当前距离正好合适，它会等待直到测量距离变远或变近。

距离仅仅是由机器人和其它自动化机器需要控制一种数值之一。当一个机器被设计用来自动维持某一数值，比如距离、压力或液位等，它一般都包含一个控制系统。这些系统有时由传感器和阀门组成，或者由传感器和电机组成。在机器人里面，由传感器和连续旋转电机组成。还必须有某些处理器可以接受传感器的测量结果并把它们转化为机械运动。必须对处理器编程来基于传感器的输入做出决定，从而控制机械输出。闭环控制是一种常用的维持控制目标数据的方法，它很好地帮助机器人保持与一个物体之间的距离。闭环控制算法类型多种多样，最常用的有滞后、比例、积分以及微分控制。

3.6.2 系统控制原理

图2-10所示的方框图描述了机器人用到的比例控制过程的步骤，即机器人用右边的IR LED/探测器探测距离并用右边的伺服电机调节机器人之间的位置以维持适当的距离。

图2-10 右边伺服电机及IR LED/探测器的比例控制方框图

仔细观察一下图2-10中的数字，学习一下比例控制是如何工作的。这个特殊的例子是右边的IR LED/探测器和右边的伺服电机的比例控制方框图。设定位置为2，说明你想机器人维持它和任何它探测到的物体之间的距离是2。测量的距离为4，距离太远。误差是设定值减去测量值的差，即2-4=-2，这在圆圈的左方以符号的形式指出，这个圆圈叫求和点。接着，误差传入一个操作框。这个操作框显示，误差将乘以一个比例常数Kp。Kp的值为70。该操作框的输出显示为–2×70 = –140，这叫输出校正。这个输出校正结果输入到另一个求和点，这时它与电机的零点脉冲宽度1500相加。相加的结果是1360，这个脉宽可以让电机大约以3/4全速顺时针旋转。这让机器人右轮向前、朝着物体的方向旋转。

第二次经过闭环，测量距离可能发生变化，但是没有问题，因为不管测量距离如何变，这个控制环路将会计算出一个数值，让电机旋转来纠正任何误差。修正值与误差总是成比例关系，该误差就是设定位置和测量位置的关系的偏差。

控制环都有一组方程来主导系统行为。图2-10中的方框图是对该组方程的可视化描述方法。下面是从方框图中归纳出来的方程关系及结果：

Error

Right distance set point – Measured right distance = 2 – 4

Output adjus

error·Kp = –2 ·70 = –140

Right servo output

Output adjust + Center pulse width = – 140 +1500 = 1360

通过一些置换，上面三个等式可被简化为一个，提供你相同的结果：

Right servo output = (Right distance set point – Measured right distance)

Kp+ Center pulse width

代入数值，你可以看到结果一致：