第十七届全国大学生智能汽车竞赛 国一（智能视觉组）技术报告

摘 要

本文是第十七届全国大学生智能车竞赛智能视觉的技术报告。报告介绍了 我们关于智能车整体设计过程，我们采用了智能车的 M 车模，处理器是 NXP 公司的 i.MX RT1064 。首先采用 openart 摄像头看坐标点图纸确定坐标，然后通 过编码器积分，再用以麦轮的姿态结算，全向移动，让小车移动到图片大致坐 标，然后利用总钻风二值化处理图像进一步姿态调整，将其移动至视野中心，此时再次利用 openart 识别图像，确定类别后机械臂搬运至指定图像框内，将所 有图片搬运成功后将其倒入指定的区域，最后返回车库。

关键词 ： openart 摄像头 麦轮 姿态解算 图像处理 机械臂

引 言

全国大学生智能汽车竞赛是一项以“立足培养、重在参与、鼓励探索、追求卓 越”为指导思想，面向全国大学生开展的具有探索性的工程实践活动。它以设计制 作在特定赛道上能自主行驶且具有优越性能的智能模型汽车这类复杂工程问题为任 务，鼓励大学生组成团队，综合运用多学科知识，提出、分析、设计、开发并研究 智能汽车的机械结构、电子线路、运动控制和开发与调试工具等问题，激发大学生 从事工程技术开发和科学研究探索的兴趣和潜能，倡导理论联系实际、求真务实的 学风和团队协作的人文精神。 我们采用了智能车的 M 车模，处理器是 NXP 公司的 i.MX RT1064 。利用 openart 对坐标纸和图片进行识别，对麦轮进行姿态解算利用编码器积分的方式到点坐标，辅助以总钻风数字图像处理进行姿态调整。在机械结构上面我们采 用了三个舵机组成了二轴机械臂，再用亚克力板做了三个装图片的盒子实现三 分类。

目录

1 系统总体设计 ...................................................................................................3

1.1 主要思路 ...............................................................................................3

1.2 技术方案概要说明 ...............................................................................3

2 机械系统设计与实现 .......................................................................................3

2.1 车身结构 ...............................................................................................3

2.2 机械臂的安装 .......................................................................................4

2.3 传感器设计与安装 ...............................................................................5

2.3.1 陀螺仪安装 ...............................................................................5

2.3.2 编码器安装 ...............................................................................5

2.3.3 摄像头安装 ...............................................................................5

2.3.4 主板和驱动安装 .......................................................................5

2.3.5 搬运盒子安装 ...........................................................................5

3 硬件系统设计与实现 .......................................................................................6

3.1 电路设计框架 .......................................................................................6

3.2 电源模块 ...............................................................................................8

3.2.1 主控、外设供电 .......................................................................8

3.2.2 舵机、电磁铁供电 ...................................................................8

3.2.3 btn 驱动 ......................................................................................9

4 软件系统设计与实现 .....................................................................................10

4.1 Openart 识别部分 ................................................................................10

4.1.1 识别 A4 纸地图 ......................................................................10

4.1.2 路径规划 .................................................................................10

4.1.3 识别图片 .................................................................................11

4.2 控制算法介绍 .....................................................................................14

4.2.1 PID 速度环 ...............................................................................14

4.2.2 PD 角度环 ................................................................................15

4.2.3 PD 位置环 ................................................................................15

4.2.4 串级 PID 控制 .........................................................................16

4.3 摄像头 pid 闭环控制 ..........................................................................16

4.4 麦轮姿态解算 .....................................................................................17

4.5 陀螺仪优化 .........................................................................................18

4.5.1 零飘检测 .................................................................................18

4.5.2 均值滤波 .................................................................................18

4.5.3 偏航角获取 .............................................................................18

4.6 串口通信 .............................................................................................18

4.7 机械臂控制 .........................................................................................19

5 结语 .................................................................................................................20

6 参 考 文 献 ...................................................................................................21

7 附录 .................................................................................................................22

7.1 主函数代码 .........................................................................................22

1 系统总体设计

1.1 主要思路

(1) 识别方面：采用 openart 对坐标纸和图片识别，保存结果并通过串口发送给 i.MX RT1064 单片机。

(2) 机械结构方面：为了较快地搬运图片，我们用三个舵机组成了一个二轴机械 臂，再用三个亚克力板做的盒子存放搬运图片。

(3) 控制车模方向：麦轮安装为“ O ”型，对四个轮子进行姿态解算，算出每个 轮子转动时受力方向，最终算出合力大小和角度，锁定偏航角为 0 ，实现移动。

(4) 控制车模速度：采用 PID 算法，对速度进行闭环控制，利用九轴陀螺仪得到当前车身角度，再利用 PID 算法形成角度环。到点坐标采用的是位置式 PID 控制，将坐标分解为 X 和 Y 轴两个向。角度环和位置环 PID 输出叠加到电机速度环上，形成多级 PID 算法控制。

1.2 技术方案概要说明

(1) 硬件方面：考虑到识别的准确性与光照有密切相关，我们做了包括主板、驱动板、补光板三块 PCB 。对于机械臂我们自己搭了一个云台，提高机械臂底部舵机的稳定性，舵机型号采用的DS3115 ， 15kg 的舵机满足了载重上的需要也保证了搬运速度的要求。装图片的盒子是亚克力板切割的，厚度为 2mm ，一定程度上减少了车身重量。由于车身前方有一个机械臂，小车的总体重心前移，为保证轮胎受力，我们在车身后方加入了两块配重。

(2) 软件方面：首先是 openart 识别坐标，图像处理像素点是有很大误差的，想要通过图像的像素准确得到点坐标非常困难，我们采集了多组数据，将数据放入 matlab 进行拟合，最终得到拟合方程，针对后面每一张图片都适用。小车控制方面主要就是，用到了角度环、位置环与速度环形成多级 PID 控制，到坐标点大致区域之后利用总钻风进行姿态调整，姿态调整也用到了 PID 控制，输出值同样叠加到速度环，串级控制。

2 机械系统设计与实现

2.1 车身结构

图 2.1 车模俯视图

图 2.2 车模前视图

图 2.3 车模侧视图

车身结构如上图所示，我们选择 M 车模，具有更坚硬的地盘能承受所设计的车身架构，M 车模相比较 H 车模而言运动更加丝滑，没有卡顿，轮子转动时噪音更小，更流畅。一个车轮由 9 个轮辊组成，轮辊采用优质耐磨硅胶材质，每个轮辊两端各镶嵌一个 3 6 2.5mm 轴承（整车共 72 个轮辊轴承），保证每个轮辊转动顺滑。车轮整体与传动轴采用硅胶材质的减震垫锁紧连接，能有效减小

车模运行时的颠簸和震动，使车模运行更加平稳；电机为 RC-380 ，驱动电压为 9V ，转速达到 18500rpm ，电机电刷采用进口碳晶，能有效增加电机使用寿命。电机内部经过动平衡加工工艺处理，电机转动震动更低，运行更平顺。

2.2 机械臂的安装

舵机选型为 DS3115 ，舵机电压为： DC4.8~6V ， 15kg 的驱动重量，在满足捡图片的同时也达到了速度的要求，角度均为 270 度，能满足向后搬运所打的 角度。机械臂主要由三个舵机和一个云台组成，三个机械臂分别为肩部、肘 部、腕部，云台安装在肩部和肘部之间，这样在肩部运动的时候机械臂整体晃 动不会很大，云台带给机械臂的稳定性提升很大，唯一的缺点是太重，同时也占车身很大地方。机械臂顶端用一根长 110cm 的 M4 螺丝钉连接一个电磁铁， 方便吸取图片搬运。

2.3 传感器设计与安装

2.3.1 陀螺仪安装

陀螺仪采用 IMU963Ra 九轴陀螺仪，安装在车身尾部，由于麦轮车的转动是以车身中点为旋转心的，因此陀螺仪安装位置对角度反馈的即时性没有很大的印象，读取偏航角要求陀螺仪尽可能水平，但是朝向没有任何要求。

2.3.2 编码器安装

四个麦轮用到了四个编码器，藏于车身中间隔板以下。

2.3.3 摄像头安装

我们使用的两个摄像头分别是 openart 和总钻风。 Openart 安装在车头，垂直镜头朝下，镜头为 2-12mm 主动变焦摄像头，由于我们整体车身比较高，采用传统的定焦摄像头在识别图片的时候可能会因为距离过远而导致识别错误，因此我们换了变焦摄像头，保证识别的准确性，但是视野因此也会变小，所以 摄像头必须安装在识别图片的正上方，即舵机电磁铁姿态调整后最终打下去的点正上方。另一个摄像头总钻风安装时略微倾斜，目的是为了增大视野，确保车模到达坐标点大致位置后能够稳定看到图片并进行姿态调整。

2.3.4 主板和驱动安装

主板安装在车身中间隔板下方，用四根铜柱固定在车模底盘，但是离底盘还差一段空隙用来放置驱动电池。驱动板分开做的两块，分别安装在车模前端和后端。

2.3.5 搬运盒子安装

三个搬运用的盒子用的是亚克力板切割拼装而成的，厚度是 2mm ，但是三个装上后还是比较重的。在每个盒子后面打一个小孔，利用 3D 打印做了一个轴承和盒子连接的装置，装上轴承后，盒子就能 360 度旋转了，将其固定在车身中间的隔板上。此外在盒子的边上连接了长杆，目的是为了能利用机械臂将盒子翻转，方便倒出图片。

3 硬件系统设计与实现

3.1 电路设计框架

图 3.1 电路设计框架

图 3.2 主控电路

图 3.3 驱动电路

图 3.4 补光板电路

3.2 电源模块

3.2.1 主控、外设供电

电源是智能车的动力源泉，如果动力源泉不稳定的话，难免会造成各种模块工作不正常。主控，外设的供电，考虑到简洁以及市场电源芯片的普遍性，我选择了 LM2940 先将 7.4V 降压到 5V ，然后再用 RT9013-33 将 5V 降到3V3，值得注意的是， RT9013-33 有一个使能端口，正常这个使能端口是连着输入端默认高电压的，RT1064 将这个使能端连在芯片的 CR 引脚上，所以在主板上电后，芯片会先上电，带内部上电成功后，再让 CR 引脚置高电压，主板外设才上电，从而一定程度上保护了主板芯片和外设。

图 3.5 3V3 稳压电路

图 3.6 5V 稳压电路

3.2.2 舵机、电磁铁供电

我们采用 AS1015 搭建的可调舵机稳压电源，用于将电源电压降压给舵机供电。

图 3.7 舵机电磁铁稳压

3.2.3 btn 驱动

我们采用 BTN7971b 芯片，这款芯片输出电流比较高，芯片耐压也比较，重要的是非常好设计，且要比同等的 DRV 驱动电路好焊接。

图 3.8 BTN 原理图

图 3.9 电机驱动 PCB 图

4 软件系统设计与实现

4.1 Openart 识别部分

4.1.1 识别 A4 纸地图

使用 openart 拍一张图片，在这张图片中使用自带的找矩形框的函数find_rects，当矩形长宽满足一定条件时，在确定黑色阈值后对矩形框内使用找色块函数 find_blobs 得到色块中心的像素坐标 x 与 y 。因为 A4 的地图与实际上的地图是等比例缩小的，算出黑点圆心到矩形长宽的距离与原本矩形长宽的比值，即可得到 A4 中黑点的坐标，乘以相应的比例系数得到实际场地图片摆放坐标。在算矩形长宽时，调用函数 find_rects 的四个顶点像素位置返回值，算出两 点之间距离，左上角与右上角的距离，左下角与右下角的距离，两者之间的距离取平均值即为矩形的长，同理可得矩形的宽。按单位厘米算，实际地图坐标为发给裁判系统坐标的 20 倍，利用这一点，在 openart 识别地图时计算的为实际地图坐标，将 openart 定一定高度后，利用matlab 拟合点坐标测量值与真实值，得到两者之间的函数关系式，再进行点 20整数倍逼近。

4.1.2 路径规划

在得到地图上的黑点坐标后，进行路径规划，希望地图内的经过所有点的路程尽量最短且最后一个点尽量靠近右侧。将地图的长边 700 米分为 3 块，起点为(0,0) 点，对于每一块运用贪心算法，每次找最近的点进行搜寻，一块搜索完毕后，块与块的连接处也是找最近点，即第一块的最后一点，下一点是第二 块中离第一块最近的点。将路径规划完毕后的点坐标逐个发送给主板。

4.1.3 识别图片

（1）准备数据集

官方给出 2000 多张 15 小类正方向的图片，将这些图片分别顺时针旋转 90度、180 度、 270 度得到 4 个方向的图片，将这些图片按类别放入文件夹中，用python 脚本文件进行标数据集，并导入 eiq 软件，数据集分为训练集和测试集。

（2）数据增强

在 eiq 中选择 AUGHENTATION TOOL, 在数据增强中加入如下的效果：Random Blur、 Random Brightness 、 Random Contrast 、 Gaussian Noise 、 HueSaturation Value、 Horizontal Flip 、 Vertical Flip 、 Random Zoom 和 Rotate 。

并在每个效果中选择合适相应的参数，进行数值范围调节。

（3）模型训练

在 eiq 训练模型的界面中选择学习率、训练轮数、每次进入训练的数量等参数。

（4）模型验证

在模型验证的混淆矩阵中，对角线大多数为绿色，细看每个类别验证准确 率也很高。不过 eiq 中的验证准确率只是参考，实际准确率还是得在实际环境 中去验证评估，图片识别受光线、阴影等环境因素影响很大。

（5）模型量化

模型 int8 量化，导出类型为 Tensorflow Lite 。

（6）模型导入

将模型和标签文本放在 openart 的 sd 卡根目录中，记事本中的标签应与训练模型中的标签一一对应。

4.2 控制算法介绍

4.2.1 PID 速度环

PID 控制器为应用最为广泛的一种自动控制器，具有原理简单、易于实现、适用范围广等功能，作为一种传统的控制方法，参数设置得当时，PID 调节控制在温度、压力、流量等不同的现场均能达到 0.1% 甚至更高的控制效果。PID 控制器就是根据系统的误差，利用比例、积分、微分计算出控制量进行控制的，从自动控制原理的角度来讲，就是将超前校正、滞后校正、滞后- 超前校正转换为比例、积分、微分三种运算及其组合。如图为 PID 控制器原理。

图 4.1 PID 控制器原理

PID 控制器共有比例系数 Kp 、积分时间 Ti 、微分时间 Td 三个参数，进而有了比例调节、积分调节、微分调节三种调节方式，三种调节方式对系统性能都有不同的影响。

比例调节是按照比例反映系统的偏差。系统出现偏差后，比例调节产生调节作用用以减小偏差。Kp 越大，可以加快调节，减小误差，但是 Kp 过大会导致系统的稳定性下降，甚至造成系统的不稳定。积分调节是为了消除系统的稳态误差。当系统出现误差时，积分调节开始进行，直至误差消失，积分调节停止，输出一常值。Ti 越小，积分作用越强，反之 Ti 越大则积分作用越弱，加入积分调节可以使系统的稳定性下降，动态响应变慢，积分作用常常与另外两种调节规律相结合，组成 PI 调节器或 PID 调节器。

微分调节反映系统偏差信号的变化率。微分调节能够预见偏差变化的趋势，在偏差尚未形成之前，就已经被微分调节作用消除，因此能够改善系统的动态性能。Td 选择合适时，可以减少超调，减少调解时间，但微分作用对噪声干扰有放大作用，因此过强的微分干扰会导致系统的抗干扰能力下降。

4.2.2 PD 角度环

通过计算陀螺仪反馈当前车身朝向的角度值和目标角度值的差值，如图2.1，用 Proportional （比例）、 Integral （积分）参数融合出各电机需要调整的速度，将此速度叠加到速度环之上，达到通过改变目标角来实时控制车身角度的效果。

图 4.2 计算角度环的电机输出值

4.2.3 PD 位置环

通过对编码器的累加计算出车子的行驶里程，再用陀螺仪的角度计算 X 坐标和 Y 坐标，从而大致实现车子在场地中位置的确定。设置目标位置，通过Proportional（比例）、 Integral （积分）参数融合当前位置，闭环反馈出四个电机的速度。

图 4.3 车子位置图解

4.2.4 串级 PID 控制

图 4.4 串级 pid 原理图

串级 pid 原理：以速度环为基础，将其他 pid 的输出值当做电机速度环的输入，形成串级 pid ，一般来说，速度环作为基础，进入中断也就是计算输出的时间要比其他 pid 时间长一些。串级 pid 的特点：

(1) 由于内环回路的存在，改变了原来的对象特性，使内环回路对象的等效时间常数变小，所以使系统的过渡时间缩短了，控制作用更加及时；

(2) 改善了对象特征，起到超前控制的作用，有效抑制内环回路干扰，这种超前控制作用最适合干扰落在内环回路内的情况，若干扰落在外环回路时，超前作用就不明显了；

(3) 提高了系统的工作频率，使振荡周期减小，调节时间缩短，系统的快速性增强；

(4) 当模型失配时，内环控制器可以很好的抑制干扰，而外环控制器则以良好的动态性能和鲁棒性能为设计目标；

(5) 由于串级控制系统的内环是一个随动控制系统，它的设定值随着外环控制器的输出而变化。外环控制器可以按照操作条件和负荷的变化情况，不断调整内环控制器的设定值，从而保证在操作条件和负荷发生变化的情况下，控制系统仍有较好的控制效果。由于麦轮有四个轮子，因此如果仅仅用速度环控制的话，很容易出现直走不准，四个轮子各走各的情况出现。所以我们用串级 pid 控制小车的基本运动，将角度环的输出值叠加到电机的输入值上，这样就控制了小车的偏航角，基本就能走直线。将位置式 pid 叠加到电机的输入上，可以实现小车准确到某一个坐标点。将三者结合起来，再辅助以麦轮的姿态结算，很容易就能实现麦轮小车的全向移动。

4.3 摄像头 pid 闭环控制

使用逐飞科技总钻风摄像头对灰度图像二值化处理，使得图片边框在摄像头视野中呈现白色，场地背景布呈现黑色。然后对二值化图像像素点扫描，确定白色边框距离视野边界的距离。事先确定图片在车前目标位置的距离值，计算当前边框距离值和目标值的差值，用 Proportional （比例）、 Integral （积分）、Differential （微分）融合差值计算出反馈给电机的速度，从而微调车子位置使得机械臂和电磁铁可以成功吸取图片。

4.4 麦轮姿态解算

图 4.5 麦轮受力分解

4.5 陀螺仪优化

4.5.1 零飘检测

我们采用角速度积分得到角度，那么一定要进行陀螺仪零飘检测，将零飘去掉，让陀螺仪运行过程中尽可能保持稳定，静止时陀螺仪的值保持不变。初始化的时候我们取八万次陀螺仪的角速度值，再取平均值得到当下近似的零飘，之后参与积分的角速度都要减去这个零飘才能使用。

4.5.2 均值滤波

去除零飘后的数据还是不能直接使用的，通过波形图可以看到角速度值呈现锯齿状，我们采用均值滤波处理角速度，均值区间为 5 。

4.5.3 偏航角获取

将均值滤波处理过的角速度除以九轴陀螺仪灵敏度 14.285 再乘以中断时间2msdt，累加后得到角度，实际一趟比赛测试下来偏航角误差很小，如果运行时间过程或者车身重心不在车模重心位置，即车身某一块地方很重，另外一块地方很轻。我们车模由于前面加了云台和机械臂，因此必须在车尾加上配重，保证车模的重心位置，一定程度上减少了陀螺仪的误差值。

4.6 串口通信

在比赛过程中我们需要频繁地进行上下位机通信，在通信过程中发现不能直接发数字，只能发字符的 ASCII 码值，而且单次只能发送一个字符，因此需要对字符进行处理，发送时要写好特定的标志位，表明有用信

图 4.6 存储坐标

由于坐标最多是三位数，有 x， y 轴两个数，因此我们定义传过来的字符六个为一组，减去 ASCII 码 48 后，将字符转化为数字，再利用百、十、个位拼接的方式最终将坐标存入两个数组中。

图 4.7 识别图片

识别图片相比较坐标而言省去了拼凑的环节，规定好类别后，传过来直接判断即可。

4.7 机械臂控制

图 4.8 机械臂控制

我们的机械臂要控制的方向有：分别将图片放入三个盒子中，最后再将图片盒子翻转，达到倒出片的目的。并且考虑到机械结构的限制，我们需要对每一个简单的机械臂动作进行动作分解，给以准确的延时，防止其打到车模框架或者前端的摄像头。至于延时，不能用系统自带的 delay 函数，因为我们是放在 5ms 中断函数中的，因此需要定义一个 delay 变量，中断每 5ms 进一次，那么 delay 就加 1 ，用来计数，以此达到延时的效果。

5 结语

我们团队都是大二的学生，最开始接触这个比赛是从大二上开始学习 51 单片机、用 AD 画 PCB ，直到十一月份具体组别公布，团队成员一拍即合，决定尝试一个新组别，智能视觉组。三个人分工明确一个人负责摄像头，一个人负责车模控制，一个人负责硬件，但对于刚接触这个比赛的我们而言起步是非常困难的，一开始我们连接线怎么接都不会，更别提各种控制原理了。我们只能先从底层一步步搭起来，先开环转个电机，读到陀螺仪的角速度，识别一张图片等等…… 就这样一直来到大二下学期，一个寒假让我们变得有些懒散，进度异常缓慢，完全看不出有任何完赛的迹象，三月份仍在优化陀螺仪，直到三月底的时候才初步形成了定位控制，即到达指定的点坐标。显然如果按照这种进度做下去肯定只能在比赛的时候当分母，可是祸不单行，四月初学校出现了疫情，所 有学生只能呆在寝室里，不能出门，当时接到通知的时候我们硬件的同学正好在实验室里，他把实验室所有与我们组有关的东西都带回了寝室，就这样封校 的两周时间内，课程学习结束外我们就语音讨论完赛方案，也是在这段时间，我们焦虑的心情仿佛平静了下来，四处求助之前的学长学姐们，甚至加了外校的前辈们询问经验以及控制算法，于是在这短短的两周内，我们完成了陀螺仪 校准，串级 pid 控制以及车模定位等等。等到一解封就迫不及待地去场地里实践，效果非常显著。

在解封到暑假前的日子里，我们按照逐飞的完赛方案一直做着，可是最终发现场地大了之后误差是在太大，不得以又陷入低谷，一直持续到暑假。偶然的机会我们看到了浙江大学的机械臂搬运方案，心血来潮想着我们也要做一个机械臂完成三分类，在这期间又看到很多麦轮姿态结算全向移动缩短时间的，我们不断学习到处询问，最终实现了全向移动，并将机械臂结合云台安装到了车模身上。临近比赛的两周时间，我们每天都能看到第二天的日出，齐心协力，终于在出征省赛的前一天晚上完赛了，那一天晚上我们不知道换了多少张地图，完赛了多少次，兴奋、喜悦不断在心头涌现，仿佛这半年时间付出的一切都是值得的。总之，对于刚入门的新手而言，智能车的门槛还是比较高的，一定要询问之前的学长学姐们，缩短入门时间，减少错误产生。此外，这次比赛告诉我 们，没有难的项目，只有不勤快的学生，任何组别，虽然有难易之分，但只要认真肯花时间在这个上面，最终会收获到意想不到的惊喜。此外这个比赛也告诉我们不到最后一刻绝不放弃，比赛前一晚上才刚刚稳定完赛，其实是有一些慌张的，没有准备提速方案，只有一套完赛代码就应对省赛，但好在结果是好的。长风破浪会有时，直挂云帆济沧海，愿每一个参加智能车竞赛的同学们都

能学有所成，学有所获。

6 参 考 文 献

[1] 隋金雪，杨莉，张岩编著 . “恩智浦”杯智能汽车设计与实例教程 . 北京；

[2] 电子工业出版社， 2018 沈长生．常用电子元器件使用一读通 [M] ．北京 . 人民

邮电出版社． 2004

[3] 邵贝贝 . 单片机嵌入式应用的在线开发方法 . 北京 - 清华大学出版社， 2004 年

10 月第 1 版

[4] 张伟等． Protel DXP 高级应用 [M] ．北京 . 人民邮电出版社． 2002

[5] 李宁，刘启新 电机自动控制系统 北京 - 机械工业出版社， 2003

7 附录

7.1 主函数代码

#include "headfile.h"

#include "isr.h"

int main(void)

{

DisableGlobalIRQ();

board_init();

systick_delay_ms(300);

button_init();

// 拨码开关初始化

mt9v03x_csi_init();

// 摄像头初始化

imu963ra_init();

// 九轴陀螺仪初始化

ips114_init(); //ips 显示屏初始化

Pid_init_all();

// 速度环 pid 初始化

PWM_and_ENCODE_init();

// 电机 pwm 、编码器初始化

uart_init_all();

// 串口 1 、 4 、 8 初始化

pwm_init(PWM2_MODULE1_CHA_C8,50,1200);

pwm_init(PWM4_MODULE3_CHA_C31,50,1600);

pwm_init(PWM4_MODULE2_CHA_C30,50,2735);

// pwm_init(PWM2_MODULE1_CHA_C8,50,5814);

// pwm_init(PWM4_MODULE3_CHA_C31,50,1750);

// pwm_init(PWM4_MODULE2_CHA_C30,50,3900);

gpio_init(B23,GPO,0,GPIO_PIN_CONFIG);// 电磁铁 IO 口初始化

systick_delay_ms(800);

// 等舵机初始化完成

pit_init();

// 定时器初始化

pit_interrupt_ms(PIT_CH0,1); //1ms 定时器

NVIC_SetPriority(PIT_IRQn,0);// 优先级为 0

EnableGlobalIRQ(0);

// 总中断打开

systick_delay_ms(300);

while(1)

{

zero_error_init();// 陀螺仪零飘自检测

display();

if(mt9v03x_csi_finish_flag==1)// 采集到一帧

{

mt9v03x_csi_finish_flag=0;

erzhihua();

ips114_displayimage032_zoom(*p,188,120,240,135); 附录

23

if(X_location[point_num]<60&&Y_location[point_num]>440)// 图

片在左上，找右下角

search_rx_point();

else if(X_location[point_num]<60&&Y_location[point_num]<60)//

图片在左下，找右上角

search_rs_point();

else

if(X_location[point_num]>640&&Y_location[point_num]>440)// 图片在右上，找左

下角

search_lx_point();

else

if(X_location[point_num]>640&&Y_location[point_num]<60)// 图片在右下，找左

上角

search_ls_point();

else

if(X_location[point_num]>60&&X_location[point_num]<640&&Y_location[point_n

um]<60)// 图片在下，找左上

search_down();

else

if(X_location[point_num]>60&&X_location[point_num]<640&&Y_location[point_n

um]>440)// 图片在上，找左下

search_up();

else

if(X_location[point_num]<60&&Y_location[point_num]<440&&Y_location[point_n

um]>60)// 图片在左，找右上

search_left();

else

if(X_location[point_num]>640&&Y_location[point_num]<440&&Y_location[point_

num]>60)// 图片在右，找左上

search_right();

else

{

pos_move_margin();

pos_straight_margin();

}

}

}

}