智能循迹小车详细制作过程

Four short words sum up what has lifted most successful individuals above the crowd: a little bit more.-author-date智能 循迹小车详细制作过程智能 循迹小车详细制作过程第二届“飞思卡尔”杯全国大学生 智能汽车邀请赛技术报告-目录第一章引言.11.1智能车制作概述 .11.2参考文献综述 .11.3技术报告内容与结构 .1第二章设计方案概述 .32.1总体设计 .32.2具体方案 .32.2.1道路识别模块 .32.2.2速度检测模块 .4第三章模型车整体设计 .53.1机械部分的调整 .53.2传感器设计与安装 .53.2.1光电管安装： .53.2.2摄像头安装： .63.2.3测速装置 .7第四章硬件电路设计 .94.1整体介绍 .94.2各模块电路介绍 . 10第五章控制算法实现 . 155.1总体软件设计 . 155.2路径识别算法 . 165.2.2基于光电管的模糊控制算法 . 165.2.2基于CMOS的算法 . 185.2.3两者的结合 . 205.3速度控制算法 . 20第六章调试及主要问题解决 . 236.1调试工具 . 236.2调试过程 . 246.3主要技术参数说明 . 25 第七章结论.27 附录A参考书目. I附录B部分程序. II第一章引言1.1智能车制作概述本队在小车制作过程中，先对比赛内容，要求与规则进行了详细分析，然后 按照要求制订了几种设计方案，并对几种方案进行比较敲定最后方案。根据方 案完成小车的总体设计和详细设计（包括底层硬件设计和总体软件设计），在完 成 了车模组装和改造后，完成了各个模块的硬件电路设计与安装，并进行了控制 算法的设计和软件实现，最后进行了整车的调试和优化。1.2参考文献综述方案设计过程中参考了一些相关文献，如参考文献所列。例如文献 1 与 2 单片机嵌入式系统在线开发方法。文献 3 与 4 是计算机控制技术，参考了其中 PID 控制策略。文献 5 到 8 是介绍了微处理器 MC9S12DG128 芯片。文献 9 到11 介绍了 CCD 图像传感器的应用和一些数据处理方法，等等。1.3技术报告内容与结构本文的主要内容框架如下： 第一章：引言。大概介绍了智能车的制作过程，参考文献说明和内容框架。 第二章：设计方案概述。介绍了各种方案，以及选择该方案的原因。 第三章：模型车机械调整。介绍了小车机械结构的调整和传感器的安装步骤。第四章：硬件电路设计。这部分是小车的硬件实现，主要给出了小车的总 体结构与各个模块的硬件电路设计。第五章：控制算法实现。本章详细介绍了各个方案采用的算法。第六章：调试及模型车技术参数。介绍了调试使用的工具与具体调试过程， 最后给出了整车的技术参数。第七章：总结。对整个模型车制作过程的总结，指出试验中发现的问题和 进一步改进的方向。第二章设计方案概述2.1总体设计由于赛道整体布局未知，因此先保证小车在各种不同环境下能够稳定运行， 再进行速度的提升。故稳定性是设计中首要解决的问题。常用的寻线方法主要有光电寻线和摄像头寻线，共有三种路径识别策略。 一、光电寻线：由多对红外收发管组成，通过检测接收到的反射光强判断赛道中心黑道位置。优点：电路设计简单，信息处理方便。 缺点：一对收发管只能检测一个点的信息，精度有限，前瞻距离远小。过弯减速剧烈，难以提高整体的运行速度。二、摄像头寻线：图像采集，动态拾取路径信息。 优点：信息量大，前瞻，能耗低的优点，能提前判弯减速。 缺点：受到的干扰较多，转弯时数据易丢失，处理相对复杂。三、光电管与摄像头结合寻线：兼顾了光电寻线的抗干扰能力强和摄像头寻线 前瞻性远、信息量大的特点。难点：光电管与摄像头之间的配合，两者切换的条件。2.2具体方案2.2.1道路识别模块使用了 CMOS 摄像头和单排七对红外发射接受二极管。根据比赛环境的不同可灵活选择各个方案。在光电管与摄像头结合寻线模式里，使用光电管检测的信息作为整幅图像 处理的第一行，在采集的图像干扰过多或信息量过少时切换到光电管循线的模 式。2.2.2速度检测模块速度传感器由一对光电管和贴在驱动电机齿轮上的纸质圆盘组成，即采用 的是反射式光电传感器。反射式光电传感器的光源有多种，我们选择的是较为 经济的红外发光二极管。第三章模型车整体设计3.1机械部分的调整硬件部分的合理安装是模型车能良好发挥的前提条件，这里主要介绍实际 中的几点经验。首先是舵机的安装，要想使前轮及时地反映舵机的控制，应尽量避免舵机 不动、轮子能转动的情况，具体做法是使舵机输出连接件之间配合紧密，减少 缝隙。前轮外倾角和前轮前束之间有一定的配合，前轮前束大，转向不利，电 机的负荷也变大。3.2传感器设计与安装3.2.1光电管安装：基于时间上和调试的考虑，设计使用了 7 对红外发射和接收光电管采取 “一”字形排列的方式,相邻两光电管间隔 20mm。为便于接收管接收路面反射回 的红外光，安装时使发射管和接收管互相有一个倾角。通过给接收管加上一定 长度的黑色套管减少相邻红外光的干扰，制作的光电管如图示：图 4.1光电管为提高光电管的前瞻性，安装时将其抬高置于小车前方并给 30 度的前倾 角，综合抗干扰的因素，确定使发光管距地最大不超 7cm。当采用光电管与摄像 头结合策略时光电管采集的信息作为图像的第一条线，此时光电管应出现在摄 像头的视野中，经试验最终确定安装离地高度 6.5cm。调试时观察不同接收管在黑线由远及近时的电压变化情况，确定各管的比 较电压的值，配合调整黑管长度，使得检测的数据能灵活反映黑线位置变化。 最后使光电管同处于一片白色的赛道上，将小车水平抬起到一定高度时，看到 所有接收管几乎在同一时刻检测到“黑”的状态，放下时又几乎在同一时刻检 测到全“白”的状态，此时大致调试完成，可以放到赛道上试跑。3.2.2摄像头安装：摄像头有两种，CCD 摄像头和 CMOS 摄像头。CMOS 摄像头采集的图像信 息虽没有 CCD 摄像头优越，但使用电压低，功耗少，故本设计采用 CMOS 摄像 头的方式。摄像头的安装较为复杂，要调整好各个方面。CMOS 摄像头的安装 主要包括高度调整，倾角调整。）高度调整 提高摄像头的高度，可以在不改变倾角的条件下增大摄像头可视范围，但是，随着摄像头高度的提升，小车的重心会不断上移，不利于保持车身的稳定性。在小车高速过弯时，过高的重心可能产生明显的侧滑现象，甚至翻车。因此，在保证车身稳定性的前提下，尽可能的增加摄像头的可视范围是摄像头高 度调整的目标。B）倾角调整 倾角的大小主要关系到摄像头对前方路径的前探量。倾角越大，摄像头的前瞻性也就越强，这对于高速行驶的小车有利。但是倾角过大造成外界强光射 入摄像头，增加了摄像头“失明”的风险，这就对抗干扰的算法提出了更高的 要求。所以，在保证摄像头正常工作的前提下，适当增加倾角，对提高小车的 行驶效果有利。综合以上分析，并通过不断的试验调整，最终我们的摄像头安装调试如下： 摄像头高度：36cm摄像头倾角：40 度40度40度37cm 36cm37cm36cm图 4.2实际制作的小车3.2.3测速装置主要有两种简易的测速方案：透射光栅式和光电管对管反射式。反射式传 感器制作简单，只需手工用圆规和直尺画一个反射板，焊接简单的电压比较电路即可，且效果也能很好的满足要求，因此选用光电管反射传感器。传感器传感器反射板反射板图 4.3传感器第四章硬件电路设计4.1整体介绍智能赛车控制系统采用模块化设计，系统主要由电源模块、电机及其驱动 模块、舵机模块、测速模块、道路识别模块、调试模块和 MC9S12 单片机模块几大部分构成。智能赛车整体装置结构设计框图如下：电源模块电源模块道路识舵机控制信号MC9S12单片机（信号处理和 控制算法实现 ）道路识 别模块舵机别模块舵机MC9S12单片机（信号处理和 控制算法实现 ）调试模块调试模块车速信号速度检测速度检测PWM驱动信号后轮转动电机 驱动模块电机 驱动模块电机电机图 4.1智能车整体结构图智能车控制系统根据各部件的正常工作的需要，对配发的标准车模用 7.2V、1.8A/h的可充电Ni-Cd电源。其中单片机系统，路径识别的光电传感器和速度检测 电路需要 5V电源供电，舵机使用 6V，直流电机直接由电池供电。考虑到启动和运行时产生的电压下降的现象，我们选用低压差稳压芯片LM2940，它可以在 500mA 时稳压，最小压差仅为 0.6V。舵机电源使用可调变 压器 NPC565。系统整个供电如图 4.2 所示：驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机驱动电机NCP5650舵机NCP565舵机NCP5650舵机NCP565舵机NCP5650舵机NCP565舵机7.7.2v2v镍镉电镍镉电池池7.2v镍镉电池单片机单片机单片机单片机单片机单片机LM2940LM2940LM2940LM2940LM2940LM2940LM2940LM2940LM2940LM2940LM2940LM2940摄像头或摄像头或 光电管速度检测电路摄像头或 光电管速度检测电路摄像头或 光电管速度检测电路摄像头或 光电管速度检测电路摄像头或 光电管速度检测电路光电管图4.2智能车电源分配图4.2各模块电路介绍速度检测电路系统提供 6V 和 5V 两个直流电源，分别采用低压差稳压芯片 LM2940 和NCP565 提供，电路图如下所示：图 4.3直流电源 6V 和 5V 的稳压电路直流电机的控制一般由单片机的 PWM 信号来完成，驱动芯片我们采用飞思 卡尔半导体公司的 H 桥驱动芯片 MC33887。图 4.4电机驱动电路图 4.5 同步信号分离电路统一提供的 MCU 板的引脚分配如下：调试模块的拨码开关电路：图 4.6 插座电路图 4.7 速度角度控制接口和开关接口相应光电检测速模块电路图：图 4.8 测速电路第五章控制算法实现5.1总体软件设计智能小车的控制是采用模块化的结构，基本思路是：将方向传感器采集来 的道路信息和速度传感器采集来的当前速度值经 CPU 处理，输出 PWM 信号到驱 动舵机和电机。方向控制和速度控制系统分别构成两个闭环系统。在综合控制 算法中，两者可相互影响；比如根据路径识别的结果来控制速度，使得在弯道 上慢速，而在直道上快速。方向控制上我们使用了三种方法：完全基于摄像头 的决策，基于摄像头和光电管的共同决策，完全基于光电管的决策。程序的总体结构流程图如下：开始MCU初始化MCU初始化数据变量初始化数据变量初始化参数设置（工作模式选择）参数设置（工作模式选择）模式选择(路径识别传感器选择)模式选择(路径识别传感器选择)方向控制方向控制速度控制速度控制图 5.1 总程序结构图5.2路径识别算法路径识别模块的流程图如下：参数设置(工作模式选择)摄像头决策模式图像采集与路径识别光电管决策模式参数设置(工作模式选择)摄像头光电管共同决策模式摄像头光电管共同决策模式光电管决策模式摄像头决策模式图像采集与路径识别光电管对路径的识别图像采集与路径识别光电管对路径的识别图像有有效效基于摄像头的方向控制基于光电管的方向控制有 无效 效光电管对路径的识别光电管对路径的识别基于摄像头的方向控制速度控制基于光电管的方向控制速度控制基于光电管的方向控制图像采集与路径识别基于摄像头的方向控制基于光电管的方向控制速度控制基于摄像头的方向控制速度控制速度控制速度控制图 5.2 路径识别算法流程图5.2.2基于光电管的模糊控制算法光电传感器获得的路面信息是离散、断续变化的，具有近似性、不完善性 并混杂一定的噪声，模糊控制能保证输出的连续性和平滑性并容纳这种不确定 的输入信息，产生光滑的输出控制量。同时小车的动力学模型复杂难以确定， 是一个典型的时延、非线性不稳定系统，而模糊控制并不依赖于控制系统精确 的数学模型，可以完成输入空间到输出空间的非线性映射。模糊控制器主要由三部分组成，即模糊化，模糊规则推理及反模糊化。输入隶属度函数规则库输入隶属度函数规则库输出隶属度函数输出隶属度函数精确输 量模糊输入量 模糊输出量 精确输出反模糊化出量模糊化模糊化模糊推理化反模糊化传感器传感器模糊推理化受控对象受控对象传感器传感器图 5.3 模糊控制器工作图确定两个模糊输入量为 e，ec，表示赛道位置与模型车中心位置的偏差和偏差的变化量，模糊输出量为 angle。e 为从-7 到+7，ec 为从-14 到+14，angle 为-37 至+37。各变量使用 11 个语言值N5，N4，N3，N2，N1，ZE，P1，P2，P3， P4，P5表示。相应的模糊规则库如下：表 5.1 模糊规则库eecN5N4N3N2N1ZEP1P2P3P4P5N5P5P4P3P2P1ZEP1P1ZEN1N2N4P5P4P3P2P1ZEP1P1ZEN1N2N3P5P4P3P2P1ZEP1ZEN1N2N3N2P5P4P3P2P1ZEP1ZEN1N2N3N1P4P3P2P1ZEZEZEN1N2N3N4ZEP4P3P2P1ZEZEZEN1N2N3N4P1P4P3P2P1ZEZEZEN1N2N3N4P2P3P2P1ZEN1ZEN1N2N3N4N5P3P3P2P1ZEN1ZEN1N2N3N4N5P4P2P1ZEN1N1ZEN1N2N3N4N5P5P2P1ZEN1N1ZEN1N2N3N4N55.2.2基于 CMOS 的算法由于设计中摄像头只用于采集道路的灰度信息，且黑白 CMOS 摄像头对路面 信息的采集已经完全能够达到对道路识别的精度要求，所以设计选用黑白 CMOS 摄像头，输出制式 PAL 信号，分辨率为 380 线。处理时每隔 6 行采集 1 行数据，利用行同步和场同步的间隔时间来完成图 像的处理和智能车的控制。具体为通过查询的方式等待场同步信号，而行同步 信号则通过中断方式获得，在每个行同步信号到来时，采集一定数量的点，并 利用下一个行同步中断到来之前的间隔时间，完成一定的计算量，这里的计算 主要是相对本行的偏差。CMOS 采集图像的为 60*60。通过试验发现，利用摄像头进行路径识别相对光电管有较大的前瞻性，从 而能提高车速。但是摄像头易受环境的干扰，主要是赛道周围黑色物体和光线 两类干扰，如果对这些外界干扰处理的不好，将严重影响智能车运行的稳定性， 甚至无法完成比赛。为了抵抗这两种类型的干扰，本设计分别采用了以下抗干扰策略：）针对赛道周围黑色物体的干扰，采用了一种基于连续性判断的寻线策略， 基本思想是：除了距离小车最近的一行外，每一行都以它的上一行作为本行找 线的基准位置，向两边扫描一定的像素，因为相邻两行黑线的中心位置的偏差 一定是小于某个值的，将该值适当放宽一些后，就可以保证快速的找到真正的 路径，同时把赛道周围的黑色干扰滤除。基于以上分析可知，在计算第 n 行的黑线中心位置时，必须知道第 n-1 行的 黑线中心位置，而通常的摄像头都是从远到近扫描的，为了利用这种寻线策略， 实际中必须先获得较近行的图像信息。这又有两种方案可供选择，一种常见的方案是将面阵 CMOS 的图像保存到 一个二维数组当中，等待最近的行信息获得后再进行处理。另外一种方案是， 将摄像头倒置安装，优先获取近处的行信息。综合比较两种处理方案不难发现， 第一种方案既浪费存储空间，又将所有行的寻线计算量积累在一个时间段计算（一帧图像的结束），使用的单片机在短暂的时间内可能难以完成。如果采用第二种方案，就可以边采集边计算，利用每一个行同步和其他不需采集行的时间间隙，来完成该行寻线的计算任务。这样，当所有行采完的同时，也基本完成 了图像的处理和路径的识别任务。通过将复杂的计算任务分布开工作的方式， 为单片机的其他控制和运算节约了宝贵的时间。鉴于此，实际采用了第二种方 案（路径识别效果见图 5.4）。图 5.4 路径识别效果）针对赛道反光和周围直射光线的干扰，采用了以下几种抗干扰策略。（1）由于赛道黑色引导线可能反光（如图 5.5）导致黑色引导线部分缺失， 可以采用线性拟合来对不完整的图像进行修补。图 5.6 是对图 5.5 进行修补后的 效果图。（）当图像缺失的比较严重，或者由于外界强光导致摄像头的完全失明 (如：赛场周围闪光灯的影响，正面直射光的通过赛道反射进入等)，摄像头只会 使采集到全空的白数据。这种图像就是无法修补的了，此时可以采用两种方案： 一种方案是利用上一帧有效图像对智能车进行控制，另一种方案是将小车的控 制权转交给依靠其他辅助路径识别设备的决策模块。在我们的智能车设计中综 合使用了这两种方案。图 5.5道路反光后效果图 5.6修补后效果5.2.3两者的结合这种方案是将光电管和摄像头结合起来的方法。具体方法是将光电管作为 摄像头的第一排，然后判断前面的道路信息是否有效，若无效的话此时切换到 光电管；有效的话继续用摄像头的有效信息。由于判断摄像头失效的条件还未 达到较好的状态，所以这种方法还在进一步探索中。5.3速度控制算法在速度控制上，采用的是位置型数字PID算法和PD算法结合的策略。 数字PID控制关键的两个问题是：其一，速度反馈的精度；其二，PID参数的配合。如何解决速度反馈精度的问题，关键在于提高速度检测的脉冲数的精 度，减少丢脉冲的个数。基于以上考虑，在选择处理脉冲的方法上，本设计采 用M法。即在固定周期内测量脉冲数。之所以采用M法，原因是：当控制周期较 长时，接受到脉冲数较多，对丢1、2个脉冲不敏感；当间隔周期较短和对象惯 性较大时，可认为脉冲数正比于电机转速，这样后续的地速度处理要也方便一 些，甚至可以直接用脉冲数直接代替车速参加运算。但是尽管如此，在车速很 低的时候，固定周期内测得的脉冲数很少，在这种情况下丢脉冲有较大偏差。数字PID位置型控制算式为：Tu(k)=k p e(k)+TIK e(i) +T Di =0e(k ) e(k 1) T其中 K p 称为比例增益， KI 称为积分系数， K D 称为微分系数。当速度偏差大于一定范围时，采用PD控制，以提高动态性能和缩短调试周 期，相应控制算法流程图如下：获取当前速度获取当前速度PD控制Y|速度值|速度偏差域值NPID控制PID控制PD控制Y采样周期到N输出PWM输出PWM图 5.7速度控制算法第六章调试及主要问题解决6.1调试工具系统在调试过程中使用到的调试工具如下：CodeWarrior 4.1 调试软件，Plastid2 仿真软件，BDM 仿真器，直流稳压电源， 信号发生器，数字示波器，多功能数字万用表，串口调试软件。其中串口调试软件是我们自行开发的工具，可以为摄像头采集的数据进行 分析和底层模块仿真。软件运行界面如下：图.1 串口调试软件运行界面6.2调试过程）光电管的调整 主要通过调整比较器电压和接受管上套的黑管长度，使得检测的数据能灵活反映黑线位置变化，出现相邻的光电接收管同时为 1 的状态且状态变化连续。在确定硬件部分正常工作后，再进行控制算法的验证，针对出现的问题改动规 则库和隶属度函数的形状。语言值越多，角度分得越细，但相应规则库也越大， 使得反应速度变慢，并且没有必要。通过试验给各输入变量 11 个语言值，即 11 个隶属度函数的方式。）CMOS 摄像头的调试 由于摄像头的调试较为复杂，为了缩短调试周期，自行开发了摄像头调试工具软件对摄像头采集的数据进行分析和底层模块仿真，即串口调试工具。在 高层算法上使用了清华大学的新版仿真软件 Plastid2 进行算法仿真验证。通过这 种分层调试的方法，解决了很多在赛车实际运行中无法发现的 Bug，使赛车在短 时间内达到了良好的运行效果。）电机的调整通过设定小车的速度和运行时间，可得电机开环时的相应曲线如图，采样 时间周期是 20mm，纵轴为输入 PWM 脉冲数：表 6.1速度开环响应曲线1 11 21 31 41 51 61 71 81 91 101 111 121 131 141 151 161 171 181 191 201 211 221 231 241 253002502001501005001-50可见开环时，小车加速到达稳态的时间约为 3.6s，减速需要时间约为 1s。以下是加了 PID 控制和 PD 控制后的闭环响应图，纵轴为实测速度（cm/s）：表 6.2 速度闭环响应曲线1201008060402001 12 23 34 45 56 67 78 89 100 111 122 133 144 155 166 177 188 199 210 221 232 243 254可见计入闭环控制后响应速度得到了明显提高，调节时间约为 1.2s。为了检测过弯时的性能，进行了长直道加半径为 50mm 的 90的弯道的 测试，所得数据绘图如下, 纵轴为实测速度（dm/s）：表 6.3直道加弯道速度响应曲线3025201510501 14 27 40 53 66 79 92 105 118 131 144 157 170 183 196 209 222 235 248从实际效果来看，小车能做到在入弯前开始减速，基本做到向内侧入弯，出弯约 10cm 开始加速，效果良好。6.3主要技术参数说明车模参数：长 29cm，宽 16cm，高 37cm ，重 2 kg所有电容总容量：1653.78uF传感器：方案一：光电管对方案二：黑白 CMOS 摄像头 1 个除了车模原有的驱动电机、舵机之外伺服电机个数：无赛道信息检测精度:5mm频率：50H第七章结论设计中在摄像头安装上采用的高度，34cm 和 36cm。根据实验结果看，高 度在解决周围光线带来的影响同时，也使智能车重心过高、运行不稳定。摄像 头的高度固定后，不同的倾角对应视野大小不同，太近判弯不够及时，太远干 扰较多。机械安装时应注意舵机的输出轴长度调整，尽可能提高它的最大转角。 前轮前倾角应尽量为零，以减少过弯的阻力。使用的简易直射式红外测速模块比较电压要根据不同的比赛环境进行调 试。另外由于小车在速度过快时容易侧滑出赛道，除了在控制上采取减速的策 略，试跑前用湿布擦干净车轮也是很有必要的。以下是发现的一些问题及相应的改进策略：（1）由于摄像头采集回的视频信号电压较低，通过比较器测得黑线的数值 为零，因此不便于使用浮动域值消除周围光线干扰，通过连续性算法判断轨迹 的实际位置。调整比较电压值可以抬高检测黑线的数值，以便于在算法中加入 浮动域值，这将在后续工作中进行尝试。（2）自主设计的测速装置和黑白码盘精度有限，反馈的速度值波动较大， 不宜于参数的整定，以后可以试用旋转编码器进行速度的采集。在舵机的安装 上可试着抬高它的位置，以加长输出力臂，使转向更为灵活。（3）虽然摄像头能提前判弯，但在过弧度很小的小 s 道时可以不必减速， 采用直冲的策略以节省时间，这也将在以后的工作中进行探索。附录 A参考书目1 卲贝贝著 单片机嵌入式应用的在线开发 清华大学出版社 20052 Steven F.Barrett,Daniel J.Pack 著嵌入式系统 20053 于海生等著 微型计算机控制技术清华大学出版社 19994 张艳兵等著 计算机控制技术国防工业出版社 20065 Motorola Inc. PWM_8B8C Block User Guide V01.16 14MAR 20026 模型车电机特性和 33886 驱动电路 20057 Freesclae HCS12 ECT PWMwww. F20078 Motorola Inc. MC9S12DT128 Device User Guide V02.09 15 October 20039黄永庆 基于 CCD 摄像的视觉跟踪误差信号提取广西大学梧州分校学报200510李绍民 刘任平 全自主足球机器人视觉系统的方案分析与比较 微计算 机信息 200411齐国清 胡晓初 线形 CCD 高速图像采集与处理系统 大连海事大学学报200412 清华 Freescale 单片机与 DSP 应用开发研究中心：第二届“飞思卡尔”杯 全国大学生智能车邀请赛比赛规则 200713 卓晴，黄开胜，邵贝贝等 学做智能车北京航天航空大学出版社 2007附录 B部分程序#include #include #include printp.h#pragma LINK_INFO DERIVATIVE mc9s12dg128b#define MAX 6000#define STEP 5#define HANG_DELAY 9#define LINE_NUM 36#define COLUMN_NUM 60#define BLACK_LINEWIDTH 2#define FAR_LINE_NUM 15#define NEAR_LINE_NUM 30#define STATUE_SCAN 0#define STATUE_FARGAP -1#define STATUE_NEARGAP 1#define STATUE_SPEEDGAP 2#define THRESHOLD_MAX_VARY 2/#define SPEED_LINE_GAP 3#defineOSC_CLOCK16000000L#defineREF_CLOCK8000000L#defineBUS_CLOCK24000000L#defineREFDV_VALUE(OSC_CLOCK / REF_CLOCK)- 1#defineSYNR_VALUE(BUS_CLOCK / REF_CLOCK) - 1void ECT_Init(void);void Clock_Init(void);void IO_Init(void);void Motor_Init(void);void DirectionMotor_Init(void);intGet_speed();void PWM_Speedctrl(int set_speed);void speed_control();void direction_control();Byte get_point(uchar line,uchar column);char get_gap(uchar line_num);void turnangle_judgement();void get_PathRange(); void get_threshold(); void set_paramters();unsigned char state;byte count,step;int dindex;byte flag,flag1;byte i;uchar test;uint stright_speed = 0;uint circle_rate = 0;unsigned int old_TCNT = 0; unsigned int new_TCNT = 0; unsigned int pluse_count; uchar threshold=255;uchar center_pos = 35;char start,end;char angle = 0;char last_angle = 0;uchar flag_allwhite = 0;uchar start_flag;char last_centerLINE_NUM;char gapLINE_NUM;char last_gapLINE_NUM;char gap_far,gap_near;char last_gapfar = 0;char last_gapnear = 0;char near_range,far_range;char last_near_range,last_far_range;uint s_speed_a = 145;uint s_speed_b = 150;uint s_speed_c = 155;uint s_speed_d = 160;uint circle_rate_a = 20;uint circle_rate_b = 40;uint circle_rate_c = 80; uint circle_rate_d = 100; Byte DataMAX;Byte lineCOLUMN_NUM;void Variable_Init(void)int i;for(i=0;iLINE_NUM;i+)last_centeri = 35;gapi = 0;last_gapi = 0;void ECT_Init(void)TIOS = 0xff; TSCR2 = 0x07;TSCR1 = 0x80;TIOS_IOS1 = 0; TCTL4_EDG1A = 1; TCTL4_EDG1B = 0;TIOS_IOS0 = 0; TCTL4_EDG0A = 1; TCTL4_EDG0B = 0;TIE_C0I = 1;void Clock_Init(void)CLKSEL_PLLSEL = 0x0; REFDV = REFDV_VALUE; SYNR = SYNR_VALUE;while(!CRGFLG_LOCK); CLKSEL_PLLSEL = 0x1;CLKSEL_PLLSEL = 0x1;void IO_Init(void)DDRA = 0x00; DDRB = 0xff;DDRT = 0x00;DDRK_DDK3 = 1; ATD1DIEN=0xFF;PORTB_BIT0 = PORTK_BIT0;PORTB_BIT1 = PORTK_BIT2; PORTB_BIT2 = PORTK_BIT4; PORTB_BIT3 = PORTK_BIT5;void Motor_Init(void)DDRP_DDRP1=1; DDRP_DDRP3=1;PTP_PTP1=1; PTP_PTP3=1; PWMPRCLK= 0x00;PWMSCLA = 0x06; PWMCLK_PCLK1 = 1;PWMDTY1 = 0; PWMPER1 = 255; PWMPOL_PPOL1 = 1;PWMCAE_CAE1 = 0; PWME_PWME1 = 1; PWME_PWME3 = 0;void DirectionMotor_Init(void)DDRP_DDRP5 = 1; PTP_PTP5 = 1;PWMPRCLK = 0x00;PWMSCLA = 0x03; PWMCTL_CON45 = 1;PWMCLK_PCLK5 = 1;PWMPOL_PPOL5 = 1;PWMCAE_CAE5 = 0;PWME_PWME5 = 1;int Get_speed()static int speed;speed = (7812/pluse_count)*17;return speed;void PWM_Speedctrl(int set_speed)static int e2=0,0;static int pwm_speed;static int old_pwm_speed; static byte pwm_ctrl; static int catch_speed;static int pi=0; static int ch=1; static int Kp = 5;static int Ti = 10;static int Td = 2;catch_speed = Get_speed();old_pwm_speed=pwm_speed;e0=e1;e1=set_speed-catch_speed;if(catch_speed(set_speed)*5/4)ch=1;pwm_speed=(Kp*e1+Td*(e1-e0)/2;elseif(ch=1)pi=0;pi=pi+e1;pwm_speed=Kp*e1/4+(pi/Ti)+Td*(e1-e0)/2;ch=0;elsepi=pi+e1;pwm_speed=Kp*e1/4+(pi/Ti)+Td*(e1-e0)/2;ch=0;if(pwm_speed220)pwm_ctrl = 220;elsepwm_ctrl=pwm_speed; PWMDTY1=255-pwm_ctrl;void speed_control()gap_speedline1,gap_speedline2,gap_speedline3;gap_speedcontrol= (get_gap(SPEED_LINE_NUM,STATUE_SPEEDGAP)+get_gap(SPEED_LINE_NUM-1,STATUE_SPEEDGAP)+get_gap(SPEED_LINE_NUM+1,STATUE_SPEEDGAP)/3;gap_speedfar = (get_gap(FAR_LINE_NUM,STATUE_FARGAP)+get_gap(FAR_LINE_NUM-1,STATUE_FARGAP)+get_gap(FAR_