西南交通大学-神马浮云队技术报告

1、第六届“飞思卡尔”杯全国大学生智能汽车邀请赛 技 术 报 告学 校：西南交通大学队伍名称：神马浮云参赛队员：刘博 卢永华 常赫 带队教师：蒋朝根 龙文杰关于技术报告和研究论文使用授权的说明 本人完全了解第六届“飞思卡尔”杯全国大学生智能汽车邀请赛关保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人，比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。参赛队员签名：刘博卢永华常赫带队教师签名：蒋朝根 日 期： 摘 要本文介绍了半年多来西南交通大学交大车队队员们在准备Frees

2、cale智能车大赛过程中的工作努力成果。智能车的硬件平台采用带MC9S12XS128处理器的S12 环境，软件平台为CodeWarrior IDE 5.0开发环境，车模采用大赛组委会统一提供的1：10 的仿真车模。文中介绍了智能小车控制系统的软硬件结构和开发流程。整个系统涉及车模机械结构调整、传感器电路设计、控制算法和策略优化等多个方面。为了提高智能赛车的行驶速度和可靠性，试验了多套方案，并进行升级，最终确定了现有的系统结构和各项控制参数。关键字：智能车，电磁，闭环，PID第一章 引 言61.1 研究背景介绍6第二章 技术方案概要说明72.1 智能车系统分析72.2硬件总体设计流程原理图：72

3、.3路径识别传感器部分：82.4电源模块：132.5驱动模块：152.6速度控制模块：172.7起跑线检测电路：18第三章 机械结构设计及制作203.1车模203.2车模改装后整体效果图213.3传感器排布特写213.4车身特写213.5转角舵机改装223.6后悬挂特写233.7编码器安装233.8底板加固243.9电机驱动安装253.10后轮差速调节253.11主控扩展板263.12电池改造263.13轮子的改造273.14舵机臂的改装273.15函数发生器283.16一代传感器连杆283.17横向图谱293.18比赛时的连杆30第四章 软件设计说明304.1 智能车系统软件结构设计304.

4、2 所用模块简介314.2.1 时钟模块314.2.2 PWM 模块324.2.3 串口模块334.2.4 AD模块334.2.5 ECT模块354.2.6中断模块364.3赛道提取与赛车控制364.3.1赛道提取算法设计374.3.2速度策略394.4调试环境设计405总结42参考文献43附录：其余控制程序44第一章 引 言1.1 研究背景介绍教育部为了加强大学生实践、创新能力和团队精神的培养，在已举办全国大学生数学建模、电子设计、机械设计、结构设计等 大竞赛的基础上，经研究决定，委托教育部高等学校自动化教学指导分委会主办每年一度的全国大学生智能汽车竞赛，并成立了由教育部、自动化分教指委、清

5、华大学、飞思卡尔半导体公司等单位领导及专家组成的“第一届飞思卡尔杯全国大学生智能汽车邀请赛”组委会。该竞赛是为了提高大学生的动手能力和创新能力而举办的，具有重大的现实意义。与其它大赛不同的是，这个大赛的综合性很强，是以迅猛发展的汽车电子为背景，涵盖了控制、模式识别、传感、电子、电气、计算机和机械等多个学科交叉的科技创意性比赛，这对进一步深化高等工程教育改革，培养本新意识，培养硕士生从事科学、技术研究能力，培养博士生知识、技术创新能力具有重要意义。本次比赛分为电磁、光电与摄像头三个赛题组，在车模中使用透镜成像进行道路检测方法属于摄像头赛题组，利用检测赛道中心电线产生的磁场来进行赛道识别的属于电磁

6、赛题组，除此之外则属于光电赛题组。论文中主要介绍电磁组的智能车制作。 第二章 技术方案概要说明2.1 智能车系统分析在满足大赛要求的前提下，设计的智能小车所追求的目标就是既快又稳的沿着既定的赛道奔驰，因此智能小车系统的设计主要包括以下两部分：1、完成智能小车控制器的硬件电路设计，根据大赛要求，调整和改进智能车模的机械结构，最大限度的发挥小车的性能。2、结合软件算法，使小车转向准确、稳定，能够安全通过各种弯道和十字交叉路口、坡道。3、作为能够自动识别道路运行的智能汽车，信息处理与控制算法至关重要，主要由运行在单片机中的控制软件完成。因此，控制软件的设计是智能车的核心环节。2.2硬件总体设计流程原

7、理图：MC9S12XS128是系统的控制核心。它负责处理传感器接收到的赛道信息、小车速度等反馈信息，转换为合适的控制量来对舵机与驱动电机进行控制。从而实现对转角和速度的控制，即舵机模块和电机驱动分别受控于XS128主控板，用于实现小车转向和驱动。电源管理模块主要为单片机及路径识别电路、舵机、驱动电路三大部分提供稳定的直流电源。路径识别模块由传感器、外围电路和S12的AD模块组成，其功能是获取前方赛道的信息，以供主控板作进一步分析处理。速度检测模块采用欧姆龙编码器，通过检测赛车的实时车速为赛车的车速控制提供控制量。2.3路径识别传感器部分：电磁传感器的设计主要包括：感应线圈的选择、信号选频放大、

8、整流与检测等几个方面，将会涉及到电磁场与波、高频、模电等相关学科的知识点传感器:通过传感器检测道路信息的流程为：我们所使用的电路图为：PCB图形为：实图：检测线圈采用组委会推荐的10mH 的工字型电感,并用6.8nf的谐振电容并联，使用LC 串并联电路来实现选频电路，谐振频率为f=对于实际传感器选择的说明：电感线圈我们采用的是标准化的“工字型”电感线圈，这种线圈感应面积大，灵敏度好，缺点是体积较大。实践证明这种电感线圈综合效果还不错，实际上我们只用两个水平的线圈就可以完成寻迹功能，速度和稳定性达到赛区的水平。缺点是太重，不稳定，个体之间的电感量有一定的差异。 由于电流的频率为20KHz，所以我

9、们综合考虑之后选择10mH的工字型电感和6.8nF的电容相并联后检测磁场。在不加其他元件的情况下，直接用示波器查看检测的波形（距导线8cm，竖直上方）为标准的20KHz 峰峰值为300mV 的正弦波。色环电感在比赛中也取得了不错的成绩。其优点为质量轻，性能稳定；缺点就是检测的精度不够高，感应到的电动势较工字型电感低。 如果只使用一个线圈，感应电动势 E 是位置 x 的偶函数，只能够反映到水平位置的绝对值|x的大小，无法分辨左右。为此，我们可以使用相距长度为 L 的两个感应线圈，计算两个线圈感应电动势的差值： 其中：h为传感器到导线的竖直距离l为两传感器之间的距离x为其中一个传感器到中间导线的距

10、离 对于实际传感器摆放位置的说明：1、水平方向两个平行的线圈2、水平方向两个内八字的线圈2.4电源模块：小车所使用的电源由是7.2V 2000mAh Ni-cd可充电电池组提供，而单片机系统、路径识别的传感器、编码器等均需要5V电源，伺服电机工作电压范围4V到6V，直流电机可以使用电池直接供电。由于电路中的不同电路模块所需要的工作电压和电流容量不相同，因此电源模块应该包含多个稳压电路，将充电电池电压转换成各个模块所需要的电压。5V电源模块用于为单片机系统、传感器模块等供电。常用的电源有串联型线性稳压电源（LM2940、7805等）和开关型稳压电源（LM2596、LM2575等）两大类。前者具有

11、纹波小、电路结构简单的优点，但是效率较低，功耗大；后者功耗小，效率高，但电路却比较复杂，电路的纹波大。对于单片机，需要提供稳定的5V电源，由于LM2940的稳压的线性度非常好，所以选用LM2940-5单独对其进行供电；而其它模块则需要通过较大的电流，而LM2575或LM2596-5，转换效率高，带载能力大，缺点是其纹波电压大，不适合做单片机电源，不过对其它模块供电还是能保证充电的电源。利用LM2940-5和LM2575或LM2596-5对控制系统和执行部分开供电，可以有效地防止各器件之间发生干扰，以及电流不足的问题，使得系统能够稳定地工作。我们经过以下途径对其他几个模块进行供电。（1）使用稳压

12、芯片 LM2940 稳压，输出 5V 电压，分别对单片机和速度检测供电。（2）使用稳压芯片 LM2575或LM2596-5 ，输出5V电压，对传感器进行供电。（3）使用稳压芯片 LM2940、LM7805和LM1117稳压，输出 5V 电压，对舵机供电。最终，我们所采用的电路图如下：将电源部分与主控底板部分联合：这样可以省略许多导线，是整体更简洁。同时也减少问题的发生概率。同时，XS128外部接有LED数码管、拨码开关、蜂鸣器等元器件，分别作为数值显示、使能或速度设定以及声光报警2.5驱动模块：采用两片BTS7960搭成H桥驱动电路，进行电机驱动。BTS7960是应用于电机驱动的大电流半桥高集

13、成芯片，它带有一个P沟道的高边MOSFET、一个N沟道的低边 MOSFET和一个驱动 Ic，如图 1所示。集成的驱动Ic具有逻辑电平输入、电流诊断、斜率调节、死区时间产生和过温、过压、欠压、过流及短路保护的功能。BTS7960通态电阻典型值为 16mQ，驱动电流可达 43A。为方便使用，采用两片BTS7960并联来达到全桥的效果其电路图为：MOS管驱动:2.6速度控制模块： 速度控制采用欧姆龙编码器进行速度的脉冲计数，反馈给XS128主控板，通过测量单位时间内的脉冲个数，反映当前速度，然后通过计算，给电机驱动输出相应的控制量，从而实现小车的加减速。欧姆龙编码器如下：2.7起跑线检测电路：起跑线

14、可以采用干簧管或者是霍尔元件，经过测试，霍尔元件的灵敏性和准确性都低于干簧管，因此，我们选用干簧管作为起跑线检测。干簧管型号选择常开型，尺寸2cm左右，装在车体两侧，每侧各有三个，保证能够准确检测。其原理较为简单，如下：外置控制开关：由于通过拨码开关设置的速度、舵机转角等各种控制参数，并不能很好的完全适应所有类型的道路类型，又由于组委会规则不允许修改软件部分，因此通过外置按键，可以方便的根据赛道情况，调整参数。从而提高车体的速度和稳定性。第三章 机械结构设计及制作3.1车模此次竞赛选用的智能车竞赛专用车模为北京科宇博通科技公司的TR-V12。智能车的控制采用的是前轮转向，后轮驱动方案。模型车基

15、本尺寸参数如下表表1基本尺寸尺寸轴距208mm前轮距167mm后轮距186mm车轮直径72mm逐减传动比17/803.2车模改装后整体效果图经过多次的改装比赛时我们的车辆造型。3.3传感器排布特写 经过测试我们发现三个横向排布的传感器可以很好的处理赛道的信息，不仅可使处理十字交叉，而且还可以很好的解决弯道的信息处理。3.4车身特写我们的车身结构比较紧凑，可以很好的适应各种赛道的需求。3.5转角舵机改装转角舵机应该具有合适的反应速度，而且还需要较大的力气。我们为了加快舵机的反应速度，我们增加了舵机臂的长度。同时得到了合适的力气，使舵机能够较好的控制角度的变化。3.6后悬挂特写这是比较新式的技术，

16、在区赛结束后我们决定增加后悬挂系统，实验证明我们的决定是正确的，后悬挂可以很好的配合车轮的转向，当车转向时，使后悬挂向相同的方向摆动，可以很好的配合车转向。后悬挂我们选用了S3010舵机，因为它具有较为理想的力量。3.7编码器安装编码器是智能车的必要设备，我们选用了采用光电编码器方案，并且通过一块金属支架安装在后轮齿轮处，与齿轮耦合。通过测量齿轮速率，间接测量车体的运动速率。光电编码器采用欧姆龙的E6A2-CS3C小型光电编码器，其旋转一周可以输出200个脉冲信号。将此信号输入单片机的脉冲捕捉模块可以获得编码器输出脉冲数。单位时间查看脉冲捕捉数值，并且清零即可获得车体运动速度。3.8底板加固由

17、于底盘的柔软，为了使其具有很好的结实性，我们在其夹了一片电机和车版的连接片。不仅具有了很好的柔韧性，而且是车身可以适当的左右摆动。3.9电机驱动安装电机驱动我们安装在电机的上面，可以起压住后面的作用，尽可能的使其能够压住后轮，使车子不至于侧翻。3.10后轮差速调节车在转弯时后轮的速度会不同，所以需要差速器。在后轮不打滑的情况下，差速越松，转向效果越好，但是太松的差速会导致无法提供驱动力。实际中在赛道上调整到比较平衡的值就可以了。在差速和摩擦轴承中适当加点油也可以一定程度上改善差速的性能。由于B车型后轮的V12-044滚珠的特效作用使B车的差速有很好的差速效果。3.11主控扩展板3.12电池改造

18、电池改造是为了使电源可以牢固稳定的供电，我们选用了T型头来改造电源。3.13轮子的改造我们在轮子上划刻了花纹增加轮子与地面的摩擦。前轮要增加横向的摩擦，所以前轮要刻成向前的花纹。后轮要增加镶嵌的摩擦，所以后轮刻成了横向的花纹。实验证明这样改装轮胎可以很好的增加车子的稳定性。3.14舵机臂的改装增加力臂可以自制舵机连杆。3.15函数发生器3.16一代传感器连杆在第一次制作中我们选择了铝制的支撑杆，有较强的支撑力。3.17横向图谱3.18比赛时的连杆我们选择了碳素纤维的连杆，它的质量轻，强度高，非常适合用作支撑杆。第四章 软件设计说明4.1 智能车系统软件结构设计如果说系统硬件对于智能车来说是它的

19、骨架和躯体，那么软件算法就是它的思想。软件算法的优劣直接体现了智能车辆的“智能”高低。所以软件系统对于智能车来说至关重要。首先，赛车系统通过路径识别模块获取前方黑色引导线的信息，同时通过速度检测模块实时获取赛车的速度。利用连续路径识别算法求得赛车与黑线位置的偏差，接着采用P方法对舵机进行控制，根据检测到的实时车速，结合模糊控制策略对赛车速度进行恰当的控制调整，使赛车在符合比赛规则情况下沿赛道快速前进。 本智能车软件基于Freescale CodeWarrior IDE 5.0.本章详细介绍了智能车软件控制思路、算法,并附带了某些函数实现方法。MC9S12DG128 微控制单元作为MC9S12

20、系列的16位单片机，由标准片上外围设备组成，包括16位中央处理器、128KB的Flash 存储器、8KB的RAM、2KB的EEPROM、两个异步串行通信接口、两个串行外围接口、一组8通道的输入捕捉或输出捕捉的增强型捕捉定时器、两组8 通道10 路模数转换器、一组8通道脉宽调制模块、一个字节数据链路控制器、29路独立的数字I/O接口、20路带中断和唤醒功能的数字I/O 接口、5个增强型CAN总线接口。同时，单片机内的锁相环电路可使能耗和性能适应具体操作的需要。MC9S12XS128片内资源表如图4.2 所用模块简介在整个系统设计中，用到了5个单片机基本功能模块：时钟模块、PWM 输出模块、ECT

21、 模块、串口通信模块以及普通IO 模块。根据系统实际需求，对各个模块进行了初始化配置，通过对相应数据寄存器或状态寄存器的读写，实现相应的功能。4.2.1 时钟模块S12 单片机中有四个不同的时钟，即外部晶振时钟、锁相环时钟、总线时钟和内核时钟。锁相环时钟与外部晶振时钟的倍、分频关系由SYNR、REFDV 两寄存器决定。总线时钟用作片上外围设备的同步，而内核时钟则用作CPU 的同步，它决定了指令执行的速度。时钟模块初始化程序如下：4.2.2 PWM 模块脉宽调制模块有8 路独立的可设置周期和占空比的8 位PWM 通道，每个通道配有专门的计数器。该模块有4 个时钟源，能分别控制8 路信号。通过配置

22、寄存器可设置PWM 的使能与否、每个通道的工作脉冲极性、每个通道输出的对齐方式、时钟源以及使用方式（八个8 位通道还是四个16 位通道）。PWM 模块的初始化设置过程为： 4.2.3 串口模块串口通信模块设有两个串行通信接口SCI1 和SCI0。使用时，可以对波特率、数据格式（8 位或9 位）、发送输出极性、接收唤醒方式等进行选择。另外，发送和接收可分开使能，模块中还提供了多种避免传输错误的选项。在本次设计，我们利用其中一个串口SCI0，将所需要的信息通过串口传到上位机上进行必要的处理和分析，以提高调试的效率。串口初始化部分程序：4.2.4 AD模块8/10 位精度 7 us, 10-位单次转

23、换时间. 采样缓冲放大器 可编程采样时间 左/右 对齐, 有符号/无符号结果数据 外部触发控制 转换完成中断 模拟输入8通道复用 模拟/数字输入引脚复用 1到8转换序列长度 连续转换模式 多通道扫描方式 4.2.5 ECT模块 一个16位向上带可编程预分频的主计数器. 一个16位的带可编程预分频的模数向下计数器 8个独立的定时器通道，每个通道具备输入捕捉和输出比较功能 4个8位脉冲累加器，也可设置成2个16位脉冲累加器. 通过对寄存器编程可以实现不同的功能 4.2.6中断模块4.3赛道提取与赛车控制良好的硬件设计需要有同样良好的软件配合，才能最大地发挥其功能。根据电磁车实际需要，软件设计主要包

24、括赛道提取算法设计和跟踪控制程序设计。4.3.1赛道提取算法设计从电磁传感器经AD转换获得的数据是每个传感器的电压幅值E，反映了传感器所在位置的电磁场强度，这个强度跟传感器与导线的水平距离d、导线电流I和传感器与导线竖直距离h有关。对固定的I和h，由E可以得到d，就能得到赛道位置。4.3.2速度策略速度上采用pid控制，通过不同的传感器采集回来的赛道位置来算出想要的速度，使入弯减速，出弯加速4.4调试环境设计使用CodeWarrior Development Studio for HCS12(X) V4.7作为程序编写、编译、下载、调试工具。在软件内部集成了在线调试功能，可以查看程序变量以及寄

25、存器值有效的调试手段是实现赛车功能的必然途径，也是提高赛车性能的捷径。利用高效、可靠的调试手段，往往可以达到事半功倍的效果。在小车调试过程中，我们主要使用了无线模块，来读取小车的运行状态信息，这大大方便了小车调试。另外，我们还使用了BDM头在线调试，可以查找出程序中存在的问题。BDM:赛道：5总结 经过了一个学期的调试，我的车在不断的改进，进步。但还是存在许多问题，如传感器的值不稳定，前后轮摩擦不理想 主动悬挂使用有问题 驱动发热等等。但因为时间有限我们没能很好的解决全部问题。但我们从中学到了许多东西。最后感谢蒋老师和龙老师的指导。感谢组委会的各位老师。参考文献1童诗白·模拟电子技术

26、基础·北京：高等教育出版社，20012阎石·数字电子技术基础·北京：高等教育出版社，19983陶永华，尹怡欣，葛芦生·新型PID控制及其应用·北京：机械工业出版社，19984谭浩强·C程序设计·北京：清华大学出版社，20015 卓晴,黄开胜，邵贝贝，学做智能车M.北京:北京航天航空出版社,20076 张伟等Protel DXP高级应用M北京. 人民邮电出版社20027 宋文绪，杨帆传感器与检测技术M第1版，北京：高等教育出版社，2004.1。8 贾伯年，俞朴传感器技术M第2版，南京：东南大学出版社，2000.8。9邵贝贝.单

27、片机嵌入式应用的在线开发方法.北京：清华大学出版社，2004.10李仕伯.马旭.卓晴.基于磁场检测的寻线小车传感器布局研究，清华大学，2009.1211 李发海，王岩智电机与拖动基础M第1版，北京：清华大学出版社，2005.8。12竞赛组秘书处.路径检测设计参考方案.2010.1。13竞赛组秘书处 技术组.20KHz 电源参考设计方案。附录：其余控制程序#include <hidef.h> /* common defines and macros */#include "derivative.h" /* derivative-specific definitio

28、ns*/#include "System.h" /包含设置系统的函数库#include "PWM.h" /定义电机驱动#include "AD.h" /定义AD转换#include "ECT.h" /定时中断#include "LED.h" /数码管显示#include "Angle_PID12.h" /34导向传感器计算舵机角度#include "Speed_PID.h" /调速控制/unsigned int p_num=0,t_num=0; /当前值

29、前一次值unsigned int D_Time=0; /十字线前后传感器切换延时int F_angle; /最后给舵机的角度 （用于十字线）byte cross_flag=0,F_F=0,Delay=0; /十字线、灯、十字线延时标志位int SPEED=0; /速度设置 int B_flag=0,BB_flag=0; byte x_flag=0,STL_FLAG=0,C_OUT=0;int B_SP=0,BB_SP=0,Q_flag=0;/ unsigned int abs_num(unsigned int temp_n,unsigned int temp_p);/return the re

30、al value of some very period of coder / /*主函数*/ void main(void) DisableInterrupts; SetBusCLK_64M(); /设置总线频率为64MHZ system.h IO_Init(); /IO口初始化 system.h AD_Init(); /AD初始化 AD.h Init_pulse_counter(); /脉冲累加器初始化 ECT.h PIT0_Init(); /定时器初始化 ECT.h PWM_Init(); /电机及舵机PWM初始化 PWM.h /拨码开关读速度/if(!PORTE_PE3=1)&

31、&(!PTH_PTH1=1)&&(!PTH_PTH3=1)&&(!PORTE_PE5=1) SPEED=120;B_SP=0;MMAX_SPEED=700;MMIN_SPEED=-200;SP=8;L_SP=45;else if(!PORTE_PE3=1)&&(!PTH_PTH1=1)&&(!PTH_PTH3=1)&&(!PORTE_PE5=0)SPEED=120;B_SP=10;MMAX_SPEED=600;MMIN_SPEED=-300;SP=6;L_SP=50;else if(!PORTE_PE3=1

32、)&&(!PTH_PTH1=1)&&(!PTH_PTH3=0)&&(!PORTE_PE5=1)SPEED=120;B_SP=20;MMAX_SPEED=600;MMIN_SPEED=-300;SP=4;L_SP=60;/else if(!PORTE_PE3=1)&&(!PTH_PTH1=1)&&(!PTH_PTH3=0)&&(!PORTE_PE5=0) SPEED=110;B_SP=0;MMAX_SPEED=600;MMIN_SPEED=-200;SP=8;L_SP=45;else if(!PORT

33、E_PE3=1)&&(!PTH_PTH1=0)&&(!PTH_PTH3=1)&&(!PORTE_PE5=1)SPEED=110;B_SP=10;MMAX_SPEED=600;MMIN_SPEED=-300;SP=6;L_SP=50;else if(!PORTE_PE3=1)&&(!PTH_PTH1=0)&&(!PTH_PTH3=1)&&(!PORTE_PE5=0)SPEED=110;B_SP=20;MMAX_SPEED=600;MMIN_SPEED=-300;SP=4;L_SP=60; /else

34、if(!PORTE_PE3=1)&&(!PTH_PTH1=0)&&(!PTH_PTH3=0)&&(!PORTE_PE5=1) SPEED=105;B_SP=0;MMAX_SPEED=600;MMIN_SPEED=-200;SP=8;L_SP=45;else if(!PORTE_PE3=1)&&(!PTH_PTH1=0)&&(!PTH_PTH3=0)&&(!PORTE_PE5=0)SPEED=105;B_SP=10;MMAX_SPEED=600;MMIN_SPEED=-300;SP=6;L_SP=50

35、;else if(!PORTE_PE3=0)&&(!PTH_PTH1=1)&&(!PTH_PTH3=1)&&(!PORTE_PE5=1)SPEED=105;B_SP=20;MMAX_SPEED=500;MMIN_SPEED=-300;SP=4;L_SP=55;/else if(!PORTE_PE3=0)&&(!PTH_PTH1=1)&&(!PTH_PTH3=1)&&(!PORTE_PE5=0) SPEED=100;B_SP=0;MMAX_SPEED=600;MMIN_SPEED=-200;SP=8;

36、L_SP=45;else if(!PORTE_PE3=0)&&(!PTH_PTH1=1)&&(!PTH_PTH3=0)&&(!PORTE_PE5=1)SPEED=100;B_SP=10;MMAX_SPEED=600;MMIN_SPEED=-300;SP=6;L_SP=50;else if(!PORTE_PE3=0)&&(!PTH_PTH1=1)&&(!PTH_PTH3=0)&&(!PORTE_PE5=0)SPEED=100;B_SP=20;MMAX_SPEED=500;MMIN_SPEED=-300

37、;SP=4;L_SP=55;/else if(!PORTE_PE3=0)&&(!PTH_PTH1=0)&&(!PTH_PTH3=1)&&(!PORTE_PE5=1) SPEED=95;B_SP=0;MMAX_SPEED=600;MMIN_SPEED=-200;SP=8;L_SP=40;else if(!PORTE_PE3=0)&&(!PTH_PTH1=0)&&(!PTH_PTH3=1)&&(!PORTE_PE5=0)SPEED=95;B_SP=10;MMAX_SPEED=600;MMIN_SPEED

38、=-300;SP=6;L_SP=45;else if(!PORTE_PE3=0)&&(!PTH_PTH1=0)&&(!PTH_PTH3=0)&&(!PORTE_PE5=1)SPEED=95;B_SP=20;MMAX_SPEED=500;MMIN_SPEED=-300;SP=4;L_SP=50; /elseSPEED=100;B_SP=0;MMAX_SPEED=600;MMIN_SPEED=-300;SP=6;L_SP=50;/起跑线/if(PORTA_PA7=1)STL_FLAG=1;else STL_FLAG=0; /if(PORTA_PA5=

39、1)C_OUT=1;else C_OUT=0; /if(!PORTA_PA0=0)&&(!PORTA_PA1=0)BB_SP=0;else if(!PORTA_PA0=0)&&(!PORTA_PA1=1)BB_SP=20;else if(!PORTA_PA0=1)&&(!PORTA_PA1=0)BB_SP=40;else if(!PORTA_PA0=1)&&(!PORTA_PA1=1)BB_SP=60;/EnableInterrupts; Delay_ms(200);for(;) while(!ATD0STAT0_SCF) ; /

40、等待AD转换完成 AD_GetValue(); /起跑线 /if(STL_FLAG=1)Check_Stop();/防止出界/if(C_OUT=1) if(Final_AD1<20)&&(Final_AD2<20)&&(Final_AD3<20)Q_flag+; else Q_flag=0; if(Q_flag>=7000)Q_flag=7000; if(Q_flag>=7000) PWME_PWME5=0; PWME_PWME1=0; / 十字线 if(x_flag=0) if(Final_AD4>=320)cross_fl

41、ag=1; if(Final_AD4<=150) cross_flag=0; if(cross_flag=1) F_angle=3000; B_flag=B_SP; else F_angle=Angle_PID12(D_value12); B_flag=0; PWMDTY23=F_angle;/上坡if(Delay=0) if(Final_AD3>=500)x_flag=1; if(Final_AD3<=300) if(x_flag=1)Delay=1; x_flag=0; if(x_flag=1|(D_Time<=50)&&(D_Time>0)

42、BB_flag=BB_SP; F_F=1; else BB_flag=0; F_F=0; / /if(F_F=1|cross_flag=1)PORTA_PA3=0,PTM=0X00;else PORTA_PA3=1,PTM=0Xff; if(speed_control_flag=1)Speed_PID(SPEED-B_flag-BB_flag),show(Final_AD3);/,/主循环for/ _FEED_COP(); /主函数main /*abs functiondo: calculate the abs of two numberschangement:return abs value

43、can be used in PACNT counting fixxing*/unsigned int abs_num(unsigned int temp_n,unsigned int temp_p) unsigned int l;if (temp_n>=temp_p)l=temp_n-temp_p;elsel=65535-temp_p+temp_n;return l; /*测速中断* #pragma CODE_SEG _NEAR_SEG NON_BANKEDvoid interrupt 66 TIME(void) /0.01秒中断1次 if(Delay=1) D_Time+; if(D

44、_Time>=200)D_Time=0,Delay=0; p_num=t_num; t_num=PACNT; /记录两次脉冲个数 now_distance +=abs_num(t_num,p_num); /计算当前行走距离 feedback_speed=abs_num(t_num,p_num); /将两次脉冲差值作为速度 speed_control_flag=1; /测速完成速度控制标志置1 PITTF_PTF0=1; /清除PIT0的标记 #pragma CODE_SEG DEFUALT /* Delay_ms(200);SPEED=28;if(!DDRE_DDRE3) SPEED=3

45、2; /拨码开关读速度if(!PTH_PTH1) SPEED=31;if(!PTH_PTH3) SPEED=30;if(!DDRE_DDRE5) SPEED=29; for(;) /Check_Stop(); while(!ATD0STAT0_SCF) ; /等待AD转换完成 AD_GetValue(); if(Final_AD1>=300)cross_flag=1; if(Final_AD1<=80) if(cross_flag=1)Delay=1; cross_flag=0; if(cross_flag=1) F_angle=Angle_PID34(D_value34); F_

46、F=1; else if(Delay=1)F_angle=Angle_PID34(D_value34),F_F=1; else F_angle=Angle_PID56(D_value56),F_F=0; PWMDTY23=F_angle; if(Final_AD2>200&&Final_AD1<100&&Final_AD1>20&&D_value56<350&&D_value56>-350) x_flag+; else x_flag=0; if(x_flag>3000) x_flag=3000; x_flag_sp=1; /1为加速直道 else x_flag_sp=0; /0为普通直道 if(Final_AD2>250)&&(Final_AD1<50)&&(D_value56<150)