2019年仓储AGV（Automated Guided Vehicle，简称AGV）企业开发的热点是从“货架到人”到“货箱到人”，2020年受全国疫情影响，又催生企业开发出医用物流AGV等新兴应用领域。不过，AGV行业如何变化和升级，都离不开“避障”这一门小而精的课题。

图1 仓储AGV图片

01 激光雷达在AGV应用的避障方法
市场上关于AGV避障的方法多种多样，各有优劣。本文主要介绍北醒激光雷达应用于AGV避障的两种主要方法：单点定向避障和固态3D面阵定向避障两种。

点击查看大图

单点定向避障
北醒TF系列单点激光雷达避障模块是基于ToF（Time of Flight时间飞行原理）的激光测距技术，用于0.1m-12m近距离的障碍物探测与避障功能。原理和使用方法都比较简单，雷达输出最近障碍物的距离值，从而指导系统进行刹车或减速的指令，是移动机器人/AGV近距离低矮障碍物探测的低成本首选可靠方案。

主要特点如下
测量精度高，感知稳定
视场角小，准直度好，避免误触发
100-1000Hz数据刷新率，灵敏避障
多通讯接口可选，即插即用
耗电量极低，重量轻，易集成
不产生回波干扰、地面不平等误报现象

图2 单点定向避障激光雷达示意图

图3 上位机测距显示图（圆圈处有障碍物）

固态3D面阵定向避障
北醒CE30 3D面阵固态激光雷达也是一种定向避障方式，不同于单线及多线扫描避障方式。我们熟知的单线和多线旋转雷达均安装于AGV顶端，实现360°或特定角度避障，而CE30通过发射一束椭圆锥形光束，探测出固定视线范围内的障碍物信息，是低矮水平面且固定路径导航AGV避障的首选低成本方案。

主要特点如下
1°角分辨率高，精确识别障碍物位置
自定义避障模式和二次开发避障模式可选
20Hz初始探测频率，避障响应更快
小体积，自带安装孔，可直接嵌入机器
内部无旋转部件，结构可靠稳定性一致
不产生回波干扰、地面不平等误报现象

图4 3D面阵定向避障激光雷达示意图

图5 自定义避障模式示意图（图中矩形区域）

自定义避障模式是专门为AGV避障应用开发的模式。如图5所示，该模式下，CE30 将会为AGV筛选出最关键的避障目标，也就是警戒区内目标A，将此目标的距离信息提供给AGV。可根据客户需求定制避障区域。

图6 二次开发避障模式（实际场景图（上）和雷达拍摄的深度图（中）及点云图（下）对比）

二次开发避障模式是为AGV提供障碍物的点云图，以此“描绘”出障碍物的大致轮廓。在预设情况下，CE30会输出一幅深度图和与之对应的每一个像素信号强度数据。深度图中每个像素的深度值表示该像素对应探测点到雷达前表面的投影距离。如图6所示，分别展示了普通灰度相机拍摄的实际场景图（上）、 同一位置 CE30拍摄的深度图（中）和根据深度数据绘制的点云图（下，从略高于 CE30位置观察）。

02 激光雷达在AGV及仓储应用案例

仓储AGV避障
将一个CE30激光雷达安装在仓储AGV的正前方，实时监测前方障碍物，控制AGV减速或刹车，辅助其快速存取货、智能搬运等功能。

方案优势：支持多机协作，可抗仓库中环境光的干扰，结构不易损坏，可靠稳定。

仓储叉车避障
将TFmini-Plus安装在叉车叉脚正前方，识别前方障碍物距离信息反馈给叉车，叉车系统进行处理后，进行防撞或辅助定位托盘位置。

方案优势：工控行业深耕时间长，针对客户非标定制，匹配度高，在低成本避障方向，性价比高。

立体仓库避障
将两个TFmini Plus分别安装在智能穿梭车的前后方，实时监测前后方障碍物及距离信息，控制小车减速或急停，从而实现存取货、搬运、避障等功能。

方案优势：可直接安装在小车上，安装便捷，快速调试，性价比高，避障精准。

一、文档说明

气压计作为目前飞控高度测量的主要传感元器件，虽然满足大多数时候的定高需
要，但是精度差强人意，尤其是在高级飞行中，例如自动化起飞和着陆很难确保气压数据的准确性，为了实现高精度的测距定高（地形模式），辅助自动起飞降落（尤其是固定翼飞机），以及诸如避障等功能，可采用TF雷达连接F4使用。
本文档将介绍如何使用TF雷达与F4建立通讯。
TF雷达将作为测距仪，在F4的声呐传感器选项中显示出来。

二、设备和接线说明

TF系列雷达有TF03、TF02、TFmini-Plus、TFmini，它们都拥有UART接口的版本，可以直接与F4焊接相连，需要占用F4的一个UART接口。接线时，注意线序对应TX-RX，RX-TX。
以OMNIBUSF4 SD为例，占用UART1接口，设备接线线序如下图。

三、地面站和F4固件说明

TF雷达与F4建立通讯需要借助地面站进行配置，并且烧入对应可支持通讯的F4固件版本。此次介绍三个常用地面站进行演示，分别为：“Betaflight”、“Cleanflight”和“Inavflight”。

为了保证功能的齐全，请使用最新版本的地面站。

四、Betaflight 和 Cleanflight 地面站的调试

BF 和 CF 两个地面站的调试步骤一致，所以此次以 BF 调试为例，所用F4飞控型号为OMNIBUSF4 SD。
调试步骤如下：

五、Inavflight 地面站的调试

地面站：INAV。所用F4飞控型号为OMNIBUSF4 V3。
调试步骤如下：

六、注意事项

①TF雷达应是出厂默认的串口模式

②F4飞控端的串口应不被接收机等其他外设占用

③F4飞控应水平放置，否则会出现“-1”值

④BF和CF源码中，有“TFMINI”和“TF02”两种协议，

⑤TF03不能在INAV中适用

最后，有意者请联系：010-57456983
www.benewake.com
北醒(北京)光子科技有限公司
北醒激光雷达
电话：010-5745 6983
邮箱：bw@benewake.com

1.       体前屈自动测量仪
   

1.1 应用背景
坐位体前屈（如图1.1所示）是一种体育锻炼项目，也是中国大中小学体质健康测试项目，它的测试目的是测量在静止状态下的躯干、腰、髋等关节可能达到的活动幅度，主要反映这些部位的关节、韧带和肌肉的伸展性和弹性及身体柔韧素质的发展水平，现坐位体前屈是中国体育中考中的必考项目之一。

图1.1 坐位体前屈
1.2 应用简介
本系统为利用北醒公司TFmini Plus、Arduino-UNO板、0.91寸OLED显示模块、BY8301-16P语音模块、喇叭以及传统的坐位体前屈测量仪结合设计开发的自动测量装置。
系统作用：实现体前屈自动测量，并显示、播报测量结果，减少人为读数过程，使体育测试过程更智能化、人性化。
2. 试验设备及接线
2.1 实验设备
l Benewake TFmini Plus标版

图2.1 TFmini Plus
TFmini Plus（如图2.1所示），详细参数见TFmini Plus使用说明。
l Arduino-UNO板

图2.2 Arduino UNO
l 0.91寸OLED液晶屏

图2.3 OLED显示屏
规格：0.91寸OLED屏（如图2.3所示），128*32点阵，IIC控制接口，兼容3.3-5V。
作用：用来显示测量结果。
l BY8301-16P语音模块

图2.4 BY8301-16P语音模块
规格：语音模块如图2.4所示，其工作电压为DC 3.6-5V，支持MP3/WAV格式，支持UART串口通信控制，自带3W功放/可外接功放。
作用：用来控制喇叭，播报提示信息和测量结果。
l BY-90-1W 无源喇叭

图2.5 无源喇叭
规格：喇叭（如图2.5所示）功率为8Ω1W。
作用：播报提示信息和测量结果。
l 电脑

图2.6 电脑
如图2.6所示，作为该系统的上位机，用来编写并上传程序到UNO板。
l 传统坐位体前屈测量仪

图2.7 体前屈测试仪
l 连接线

图2.8 连接线

杜邦线——用于Plus与UNO板连接；
USB方口数据线——用于UNO板与电脑连接以及供电。
2.2 接线

图2.9 系统接线图
l TFmini Plus 线序：

图2.10 TFmini Plus线序

TFminiPlus供电电压为5V，直接连接Arduino板的5V和GND，其他雷达需查阅产品规格书，确保供电正常；
l 对于Arduino UNO板，串口通信连接需要注意：
（1） TFmini Plus的TX端接板子的pin 4口（软串口Serial2的RX），雷达的RX端接板子的pin 5口（软串口Serial2的TX），
（2） 语音模块接UNO板的5V和GND，TX端接板子的pin 2口（软串口Serial1的RX），雷达的RX端接板子的pin 3口（软串口Serial1的TX），软串口1定义在voice库中，语音模块只能按照顺序接UNO板的2、3引脚，否则必须修改voice库文件（支持设置音量0-20）；
l 0.91寸OLED显示屏连接5V（或3.3V）和GND供电， SCL和SDA接UNO板的SCL和SDA；

3.         体前屈自动测量原理
   

图3.1 体前屈自动测量系统示意图

图3.2 体前屈自动测量工作原理流程图
如图3.1和图3.2所示，为体前屈自动测量的系统示意图和工作原理流程图。
首先，我们包含voice和oled库文件，定义变量，对串口、语音模块和OLED显示屏进行初始化；读取雷达数据，对系统进行标定，获得start=dist1,zero=dist0;
然后，读取数据，判断位置初始化状态init_stat和雷达数据与start的关系，如果不满足要求，获取数据再进行判断，如果满足要求（!init_stat（没有初始化） && D==start（雷达数据与标定的初始位置相等）），语音模块播报“ready” ，开始测量；
测量过程中，继续判断位置初始化状态init_stat和雷达数据与start的关系，如果不满足要求，获取数据再进行判断，如果满足要求（init_stat（位置初始完毕） && D！=start（挡板离开初始位置））；时刻检测挡板是否移动，如果挡板静止，大约经历2.3s，系统测距稳定，播报和显示测量结果；
最后，判断是否需要进行下一次测试，如果不需要，断电，结束测试。

4.         系统程序编写
   

该例程需要添加OLED的库和自定义的库voice和oled。
file:///C:/Users/ADMINI~1/AppData/Local/Temp/msohtmlclip1/01/clip_image035.jpg
##include “voice.h”
#include “oled.h”
#include<SoftwareSerial.h>//软串口头文件
SoftwareSerial Serial2(4,5); //定义软串口名称为Serial2，并把pin2定为RX，pin3定为TX
/对于有多个串口的arduino板如DUE板，注释掉上面两段代码，直接使用Serial1串口/
/雷达基本数据/
int check;//校验数值存放
int uart[9];//存放雷达测量的数据
int k;
const int HEADER=0x59;//数据包帧头
/体前屈自动测量数据/
int D;//雷达实测距离值
int d=0;//上一个距离值
int result=0;
const int zero=40;//标定的0点值，安装完成后需要进行标定设置
int VAR[23]={0};
int var;
int num=0;
const int start=54;//标定的起点值，安装完成后需要进行标定设置
int CHA[15]={0};
int cha=0;
int count=0;
boolean init_stat=false;//判断是否进行位置初始化
void setup()
{
Serial.begin(115200);//设置arduinouno与电脑连接串口的波特率
Serial2.begin(115200);
oled_begin();//OLED显示模块初始化
voice_begin();//语音模块初始化
voice_setvolume(14);//设置喇叭音量
}
void loop()
{
Serial2.listen();
if(Serial2.available())//查看串口是否有数据输入
{
if(Serial2.read()==HEADER)//判断数据包帧头0x59
{
uart[0]=HEADER;
if(Serial2.read()==HEADER)//判断数据包帧头0x59
{
uart[1]=HEADER;
for(k=2;k<9;k++)//存储数据到数组
{
uart[k]=Serial2.read();
}
check=uart[0]+uart[1]+uart[2]+uart[3]+uart[4]+uart[5]+uart[6]+uart[7];
if(uart[8]==(check&0xff))//按照协议对收到的数据进行校验
{
D=uart[2]+uart[3]*256;//计算距离值
if(!init_stat&& D==start)
{
cha=D-start;
for(k=15;k>=1;k–)
{
CHA[k]=CHA[k-1];
}
CHA[0] =cha;
for(k=0;k<15;k++)
{
if(CHA[k]==0)
{
count++;
}
}
oled_measuring();
if(count>=15)
{
voice_ready();
init_stat=true;
}
}
if(D!=0 &&D!=start && init_stat)
{
var=abs(D-d);
for(k=23;k>=1;k–)
{
VAR[k]=VAR[k-1];
}
VAR[0] =var;
for(k=0;k<23;k++)
{
if(VAR[k]==0)
{
num++;
}
}
if(num>=23)
{
result=zero-D;
oled_result(result);
voice_num(result);
Serial2.listen();
init_stat=false;
}
d=D;
num=0;
}
}
}
}
}
}
5. 系统工作流程和数据查看
5.1 工作流程图

图5.1 系统工作流程图
l 系统安装完成后，供电，上传程序，校准起始点和0位置；
l 挡板置于起始点，等语音模块播报“ready”后，开始测量；
l 挡板离开起始点后，不能停顿过长时间，一般在2.3s以内，挡板静止后，稳定测量后，播报并在OLED中显示测量结果。
5.2 数据查看

图5.2 工作数据
6. 注意事项
l TFmini Plus的测量分辨率为5mm，如果测量精度为mm，会有较大误差，所以只测量到cm，后面可以用精度更高的产品，实现mm级测量。
l 体前屈自动测量仪在测量过程中，不会播报，但是，不能停顿过长时间（小于2.3s）。
l 该系统为一次一测，测量完毕后，需要重新归位到起始点。

1. 人流身高测量仪说明
   本方案为利用北醒公司产品 TFmini-Plus 和 Arduino-UNO 板结合开发出的小设备。
   其作用为：统计人流数量以及相应目标身高高度。

2. 试验设备及接线
   2.1 实验设备
   Benewake TFmini-Plus 标版
   
   TFmini-Plus 详细参数见 TFmini-Plus 使用说明。
   Arduino-UNO 板
   
   电脑
   
   电脑作为程序编写上传和数据显示功能。数据显示功能后期可进行升级优化，例如增加显示屏显示当前人流
   量和身高数据，用扬声器语音播报“欢迎光临”等问候语。
   连接线
   
   杜邦线——用于 Plus 与 UNO 板连接（Plus 端子可拆）
   USB 方口数据连接线——用于 UNO 板与电脑连接
   2.2接线
   
   TFmini Plus 产品线序定义为：红色+5V，黑色 GND，绿色 TX，白色 RX；TFmini-Plus 供电电压为 5V，所以
   直接连接 Arduino 板的 5V 和 GND 即可，其他雷达请查阅产品规格书，确保供电正常；
   串口通信连接需要注意，对于 Arduino UNO 板雷达的 TX 端接板子的 pin 2 口（软串口 Serial1 的 RX），雷达的 RX 端接板子的 pin 3 口（软串口 Serial1 的 TX），这个跟后文中的程序编写有关。

3. 人流身高测量原理
   雷达启动后，设置安装高度、身高限度、下限值。
   当人进入探测区域时，数据会出现上升，输出人数，每出现此情况，人数加 1，输出人数。
   当人处于探测区域时，数据会在身高限定以上波动，此时比较赋值 Height，Height 为人在探测区域内数据波动的最大值。
   当人走出探测区域时，数据会出现下降，输出 Height。

4. 程序框图
   

5. 程序编写
   该例程功能的实现至少需要两个串口，一个接受雷达的数据，另一个用于将数据输出到电脑端显示出来。可以复
   制以下代码粘贴到 IDE 程序编辑窗口。
   #include<SoftwareSerial.h>//软串口头文件
   SoftwareSerial Serial1(2,3); //定义软串口名称为 Serial1，并把 pin2 定为 RX，pin3 定为 TX
   /对于有多个串口的 arduino 板如 DUE 板，注释掉上面两段代码，直接使用 Serial1 串口/
   int dist;//雷达实测距离值
   int strength;//雷达信号强度，强度在 100 以下数据不可信
   int IH=230;//雷达安装高度（需设置），需要根据实际黑色物体进行校正
   int H;//身高值
   int h=0;//上一次身高值
   int check;//校验数值存放
   int i;
   int Number=0;//人数
   int Height=0;//身高高度最大值
   int up=50;//身高限定（需设置），物体身高在此值以上才有效
   int down=40;//下限值（需设置），高度在下限值以下突破身高限度才会计数，防止高度处在恰好身高限度时导
   致数据波动，产生错误数据
   int uart[9];//存放雷达测量的数据
   const int HEADER=0x59;//数据包帧头
   void setup()
   {
   Serial.begin(115200);//设置 arduino 与电脑连接串口的波特率
   Serial1.begin(115200);//设置雷达与 arduino 连接串口的波特率
   }
   void loop()
   {
   if (Serial1.available())//查看串口是否有数据输入
   {
   if(Serial1.read()==HEADER)//判断数据包帧头 0x59
   {
   uart[0]=HEADER;
   if(Serial1.read()==HEADER)//判断数据包帧头 0x59
   {
   uart[1]=HEADER;
   for(i=2;i<9;i++)//存储数据到数组
   {
   uart=Serial1.read();
   }
   check=uart[0]+uart[1]+uart[2]+uart[3]+uart[4]+uart[5]+uart[6]+uart[7];
   if(uart[8]==(check&0xff))//按照协议对收到的数据进行校验
   {
   dist=uart[2]+uart[3]*256;//计算距离值
   strength=uart[4]+uart[5]*256;//计算信号强度值
   H=IH-dist;//计算身高值
   /测量人数/
   /原理：当数据从下限值突破身高限定值则人数加 1/
   if(H>=up&h<down)//当人进入测量区域，人数加一
   {
   Number=Number+1;
   h=H;
   Height=H;
   Serial.print("Number = ");
   Serial.print(Number);//输出数量值
   Serial.print(‘\t’);
   }
   if(H>=up&h>=down)//当人处于测量区域内，通过比较得出此期间的最大值
   {
   if(Height<H)
   {
   Height=H;
   }
   }
   if(H<down&h>=up)//当人走出测量区域，输出身高最大值，同时将最大值重新置零
   {
   Serial.print("Height = ");
   Serial.print(Height);//输出身高值
   Serial.print(‘\n’);
   h=H;
   Height=0;
   }
   // Serial.print("H = ");
   // Serial.print(H);//输出身高值
   // Serial.print(‘\t’);
   // Serial.print("dist = ");
   // Serial.print(dist);//输出雷达测试距离值
   // Serial.print(‘\t’);
   // Serial.print("strength = ");
   // Serial.print(strength);//输出信号强度值
   // Serial.print(‘\n’);
   }
   }
   }
   }
   }

6. 数据查看
   将程序上传到 Arduino 板，打开串口监视器，即可看到雷达实时探测的人流数量和相应目标的身高高度，如图：
   

7. 注意事项
   安装高度需要根据实际黑色物体进行校正，因为不同颜色物体反射率不同雷达接收的信号不同。
   当多人并走时，雷达光斑只能打到一个物体，只能计数加 1，适用单人出入的场景。
   如果光斑未完全打到头顶，身高数据会出错。考虑人行走时身高会浮动，尽量让脚落在光斑左右，探测到最高值，减小误差。
   缓冲区域为“身高限定-下限值”，避免探测高度恰好在“身高限定”的物体时，数据波动产生重复人数加 1的情况。

1.试验设备及接线
1.1实验设备
• MiniQ 桌面机器人底盘

• 底盘直径：122mm
• 轮子直径：42mm
• 底盘高度：15mm
• 兼容 Arduino 标准板及 Romeo 控制器固定孔
• 电机参数：
  • N20 电机电压：3-9V
  • 无负载转速：13000rpm
  • 50：1 减速箱
  • 260rpm@6V
  • 40mA@6V
  • 360mA 堵转@6V
  • 10 盎司英寸扭矩@6V

• Romeo 三合一 Arduino 兼容控制器

• 采用 Atmel Atmega328 单片机
• Arduino UNO bootloader
• 完全兼容 Aruduino UNO 的端口布局
• 集成 APC220 无线数传和 DF-BluetoothV3(SKU:TEL0026)蓝牙模块接口
• 支持 5 组 I2C 总线接口
• 支持两路电机驱动，峰值电流 2A，4 个控制口使用跳线切换
• 外部输入电压范围：6V~20V
• 更详细的参数介绍详见附录的网页地址。

• MiniQ 小车上层安装板

• Benewake TFmini 标版

TFmini 详细参数见 TFmini 使用说明。

• 9g 舵机

1.2接线

2.小车避障原理
小车启动后，小车开始向前运动。当雷达探测到前方阈值内有障碍物时，小车停止运动，开始左右扫描寻路。舵机搭载 TFmini 从 90°开始向 180°扫描，然后从 180°向 0°扫描。

当扫描方向无障碍物时，小车向此方向转向，舵机回正到 90°。若从左至右扫描一圈都没有可以行进的路线，则小车后退，舵机回正。
逻辑流程图如下所示：

3.注意事项
• 当前避障原理模型只用来抛砖引玉，探索用 TFmini 避障的可行性，并不能大范围的适用于大规模的商业场景，如有需要，应以专业软件开发人员的代码为准。
• 搭载的外部电源过重时，会影响小车车轮的摩擦力，可能两个车轮的转速不一致，导致小车并不能按照轨迹行驶。
• 小车车轮在光滑地面有可能造成空转的现象，导致小车不能走直线。
• 如果单独对 TFmini 外部供电，则需将外部电源和控制板共地处理。
• 如果搭载更高复杂度的程序，要考虑芯片的能力，当前开发板在跑程序时已经发现会有卡顿的现象。
4.附录
4.1代码
#include <Servo.h>
Servo myservo;
int pos=90; //定义舵机角度
bool flag=true;//定义舵机转向
float dist_f;//定义 foward 方向距离
float dist_s;//定义 sideway 方向距离
int E1=5; //定义 M1 使能
int E2=6; //定义 M2 使能
int M1=4; //定义 M1 控制
int M2=7; //定义 M2 控制
int temp_distance =0;
/**

• 双轮停止
  /
  void brake(void){
  digitalWrite(E1,LOW); //给 E1 低电平
  digitalWrite(E2,LOW); //给 E2 低电平
  }
  /*
• 双轮前进
  /
  void advance(char a, char b){
  analogWrite(E1,a);
  digitalWrite(M1,LOW);
  analogWrite(E2,b);
  digitalWrite(M2,LOW);
  }
  /*
• 双轮后退
  /
  void back(char a, char b){
  analogWrite(E1,a);
  digitalWrite(M1,HIGH);
  analogWrite(E2,b);
  digitalWrite(M2,HIGH);
  }
  /*
• 左转
  /
  void turn_L(char a, char b){
  analogWrite(E1,a);
  digitalWrite(M1,LOW);
  analogWrite(E2,b);
  digitalWrite(M2,HIGH);
  }
  /*
• 右转
  /
  void turn_R(char a, char b){
  analogWrite(E1,a);
  digitalWrite(M1,HIGH);
  analogWrite(E2,b);
  digitalWrite(M2,LOW);
  }
  /*
• 读取 TFmini 测量结果
  /
  void getTFminiData(int distance, int* strength) {
  static char i = 0;
  char j = 0;
  int checksum = 0;
  static int rx[9];
  if(Serial.available()) {
  rx = Serial.read();
  if(rx[0] != 0x59) {
  i = 0;
  } else if(i == 1 && rx[1] != 0x59) {
  i = 0;
  } else if(i == {
  for(j = 0; j < 8; j++) {
  checksum += rx[j];
  }
  /*
  if(rx[8] == (checksum % 256)) {
  *distance = rx[2] + rx[3] * 256;
  strength = rx[4] + rx[5] * 256;
  }/
  *distance = rx[2] + rx[3] * 256;
  strength = rx[4] + rx[5] * 256;
  i = 0;
  } else {
  i++;
  }
  }
  }
  void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  //舵机的插口在 4
  myservo.attach(4);
  brake();
  /
• 将雷达指向前方
  /
  myservo.write(pos);
  /
• 设置轮胎电机输出口
  /
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  delay(10);
  }
  void loop() {
  /
• 读数一次
  /
  int distance = 0;
  int strength = 0;
  getTFminiData(&distance, &strength);
  while(!distance) {
  getTFminiData(&distance, &strength);
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.print("cm ");
  Serial.print("strength: ");
  Serial.println(strength);
  }
  /
• 设置 30CM 阈值
  /
  if(distance <= 30 && distance > 0){
  temp_distance = distance;
  }
  delay(10);
  /
• 判断读数距离
• 如果度数距离小于阈值，则停车，开始向左向右扫描，直到扫描出有空隙可以走，然后车轮转弯，然后扫描器回正
• 如果读数距离大于阈值，则开车
  /
  if(temp_distance <= 30 && temp_distance >= 0){
  brake();
  /
• 判断当前舵机应该向左还是向右转
  /
  if(flag){
  if(pos<170){
  pos=pos+45;
  }else{
  flag = false;
  }
  /
• 如果探测距离大于阈值，则舵机回正，小车转向
  /
  if(distance > 32){
  pos = 90;
  myservo.write(pos);
  delay(1200);
  //判断小车回正方向
  if(pos >= 90){
  turn_L(35,35);
  }else{
  turn_R(35,35);
  }
  delay(250);
  temp_distance = distance;
  }
  /
• 如果探测距离小于阈值，则继续扫描
  /
  else{
  myservo.write(pos);
  delay(1200);
  }
  }else{
  if(pos>10){
  pos=pos-45;
  }else{
  flag=true;
  }
  /
• 如果探测距离大于阈值，则舵机回正，小车转向
  /
  if(distance > 32){
  pos = 90;
  myservo.write(pos);
  delay(1200);
  //判断小车回正方向
  if(pos >= 90){
  turn_L(35,35);
  }else{
  turn_R(35,35);
  }
  delay(250);
  temp_distance = distance;
  }
  /
• 如果探测距离小于阈值，则继续扫描
  /
  else{
  myservo.write(pos);
  delay(1200);
  }
  }
  }
  /
• 如果前方没有障碍物，直行
  /
  else if(distance > 32 && distance < 1200){
  advance(35,35);
  }
  /
• 如果雷达挂了，小车停止
  */
  else if(distance == -3){
  brake();
  }
  }