湖北汽车工业学院电磁组惠捷二号技术报告.doc

个人收集整理 勿做商业用途第六届“飞思卡尔”杯全国大学生 智能汽车竞赛 技 术 报 告 学 校：湖北汽车工业学院队伍名称：惠捷二号参赛队员:刘朱徐 卢 辉 张 明带队老师： 吕 科 郭 伟 2011年8月1日关于技术报告和研究论文使用授权的说明本人完全了解第六届“飞思卡尔”杯全国大学生智能汽车邀请赛关保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人,比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。 参赛队员签名: 带队教师签名： 日 期： 目录第一章引言5第二章智能车整体设计6 2.1技术方案的实现6 2.2系统机械结构设计6 2。3智能车前轮定位的调整8 2.3.1主销后倾角8 2.3.2主销内倾角8 2.3。3车轮外倾角8 2。3。4 前轮前束9 2.4其它机械结构的调整9第三章系统硬件设计10 3.1电源部分10 3.2电机驱动电路11 3。3测速电路11 3。4检测模块13第四章系统软件设计15 4。1 主程序流程框图15 4.2 程序初始化16 4。3 速度测量16 4.4 舵机和速度的PID控制算法16 4。4。1 经典PID控制算法介绍16 4。4。2 经典PID算法在本智能车上的应用18第五章系统调试195。1基于LABVIEW的调试平台19第六章 智能车技术参数说明23 参考文献24部分程序代码25IV第一章 引言本智能小车以飞思卡尔16位微控制MC9S12XS128作为唯一的核心控制单元，采用一组线圈作为传感器，感应由赛道中心导线产生的交变磁场检测道路信息，通过单片机处理优化，把控制信号发送给电机和舵机。同时通过自制的转速传感器获取小车速度，进行速度反馈处理,最后利用 PID控制方式作为电机驱动。本文对线圈的选择及对跑道位置的的检测进行了简要的分析，对小车的硬件与软件设计进行了介绍。技术报告以智能小车的设计为主线,包括小车的构架设计、软硬件设计,以及控制算法研究等，共分为五章。其中，第一章为引言部分.第二章主要介绍了小车的总体方案的选取。第三章介绍了硬件设计，主要介绍了电路的设计；检测模块的设计.第四章对小车的软件设计进行了详细的介绍.第五章描述了小车的软件调试过程。第二章 智能车整体设计2。1技术方案的实现第五届智能车竞赛新增磁导航组，感应由赛道中心导线产生的交变磁场检测道路信息.在准备比赛的初始阶段，组委会提供的设计参考方案给我们很大的帮助。在路径检测方面，我们最终采用了使用单片机直接采样交变电压信号。其电路简单可行.与光电组和摄像头组相比，磁导航组最大的缺点是其前瞻的局限性，但采用线圈检测具有一定的稳定性且干扰小。 在线圈的选择方面,我们经过了一个长期的过程，尝试了各种不同类型的线圈作为传感器,从最初电感到磁敏二级管,最后选择了电感值为100MH的线圈且等间距布置。在起跑线识别方面,我们选择了干簧管检测。在车体左右各布置3个干簧管且相互并联，作为一路I/O且采用查询检测方式.系统主板采用主办方提供的飞思卡尔16位微控制器MC9S12SX128作为核心控制单元，采用PWM和PID技术，控制舵机的转向和电机转速。在转向控制方面，我们采用了双舵机的摇头方案，一个舵机控制前轮转向,另一个舵机控制摇头角度，这样能使小车具有很好的随线性。在速度控制方面，采用PID控制,在智能车后部安有光珊，采集关于车轮转速的脉冲信号，经由MCU进行PID计算后自动调节对电机的PWM波的占空比，控制车速度。我们还扩展了液晶和键盘模块作为人机操作界面,以便于智能小车的调试与相关参数调整.2.2系统机械结构设计2。2。1摇头方案的实现为了使小车具有较好的随线性，在转向过程中给一个合适的转向,我们选择了摇头方案。舵机带动T形摇头摆臂摆动,进而带动前面传感器支架摆动。考虑到小车经过坡道及信号强弱处理，T形摇头摆臂与传感器支架设计成可调结构，可调整传感器离地间隙.具体结构图形如下: 图2.1 摇头方案的实现2.2。2舵机的固定 舵机的固定我们考虑到以下3点: 1舵机与底盘之间的固定可靠。 2整车结构紧凑。 3尽量减轻整车质量,使用轻质量的固定材料。具体结构图如下： 图2。2 舵机的固定2.3智能车前轮定位的调整现代汽车在正常行驶过程中，为了使汽车直线行驶稳定，转向轻便，转向后能自动回正,减少轮胎和转向系零件的磨损等,在转向轮、转向节和前轴之间须形成一定的相对安装位置,叫车轮定位，其主要的参数有：主销后倾、主销内倾、车轮外倾和前束2。智能车竞赛模型车的四项参数都可以调整，但是由于模型车加工和制造精度的问题，在通用的规律中还存在着一些偶然性。2。3.1主销后倾角主销后倾角是指在纵向平面内主销轴线与地面垂直线之间的夹角2。它在车辆转弯时会产生与车轮偏转方向相反的回正力矩,使车轮自动恢复到原来的中间位置上。所以，主销后倾角越大，车速越高，前轮自动回正的能力就越强，但是过大的回正力矩会使车辆转向沉重。通常主销后倾角值设定在1到3.模型车通过增减黄色垫片的数量来改变主销后倾角的，由于竞赛所用的转向舵机力矩不大，过大的主销后倾角会使转向变得沉重，转弯反应迟滞，所以设置为0，以便增加其转向的灵活性.2。3.2主销内倾角主销内倾角是指在横向平面内主销轴线与地面垂直线之间的夹角，它的作用也是使前轮自动回正2。角度越大前轮自动回正的作用就越强，但转向时也就越费力，轮胎磨损增大；反之,角度越小前轮自动回正的作用就越弱。通常汽车的主销内倾角不大于8。 对于模型车,通过调整前桥的螺杆的长度可以改变主销内倾角的大小,由于过大的内倾角也会增大转向阻力，增加轮胎磨损,所以在调整时可以近似调整为03左右，不宜太大。主销内倾和主销后倾都有使汽车转向自动回正，保持直线行驶的功能。不同之处是主销内倾的回正与车速无关,主销后倾的回正与车速有关，因此高速时主销后倾的回正作用大，低速时主销内倾的回正作用大. 2。3。3车轮外倾角前轮外倾角是指通过车轮中心的汽车横向平面与车轮平面的交线与地面垂线之间的夹角2，对汽车的转向性能有直接影响,它的作用是提高前轮的转向安全性和转向操纵的轻便性1。在汽车的横向平面内，轮胎呈“八”字型时称为“负外倾，而呈现“V”字形张开时称为正外倾。如果车轮垂直地面一旦满载就易产生变形,可能引起车轮上部向内倾侧，导致车轮联接件损坏。所以事先将车轮校偏一个正外倾角度,一般这个角度约在1左右，以减少承载轴承负荷，增加零件使用寿命，提高汽车的安全性能。 模型车提供了专门的外倾角调整配件，近似调节其外倾角。由于竞赛中模型主要用于竞速，所以要求尽量减轻重量,其底盘和前桥上承受的载荷不大，所以外倾角调整为0即可，并且要与前轮前束匹配。2。3。4 前轮前束所谓前束是指两轮之间的后距离数值与前距离数值之差,也指前轮中心线与纵向中心线的夹角2。前轮前束的作用是保证汽车的行驶性能，减少轮胎的磨损.前轮在滚动时，其惯性力自然将轮胎向内偏斜，如果前束适当,轮胎滚动时的偏斜方向就会抵消，轮胎内外侧磨损的现象会减少.像内八字那样前端小后端大的称为“前束”，反之则称为“后束或“负前束”。在实际的汽车中,一般前束为012mm 。在模型车中，前轮前束是通过调整伺服电机带动的左右横拉杆实现的。主销在垂直方向的位置确定后，改变左右横拉杆的长度即可以改变前轮前束的大小。在实际的调整过程中，我们发现较小的前束，约束02mm可以减小转向阻力,使模型车转向更为轻便，但实际效果不是十分明显。虽然模型车的主销后倾角、主销内倾角、车轮外倾角和前束等均可以调整，但是由于车模加工和制造精度的问题，在通用的规律中还存在着不少的偶然性，一切是实际调整的效果为准。2。4其它机械结构的调整1为有效地防止由于轮胎与轮辋错位而引起的驱动力损失,我们对前轮进行了粘胎处理.2为了防止小车在高速时急转弯时发生侧滑甩尾等现象，在底盘下部车尾与车身也进行了粘接处理。3为提高小车的稳定性，我们尽量做到使小车重心降低且使小车的重心后移,如将电池后移.第三章 系统硬件设计3。1电源部分为了能使智能车系统能正常工作，就需要对电池电压调节。其中，单片机系统、车速传感器电路需要5V电压，路径识别的电路电压工作为5V、伺服电机工作电压范围4.8V到6V（或直接由电池提供），直流电机可以使用7.2V 2000mAh Ni-cd蓄电池直接供电。考虑到由于驱动电机引起的电压瞬间下降的现象，因此采用低压降的三端稳压器成为必然。我们在采用LM7806,和LM2940作为稳压芯片。经试验电压纹波小，完全可以满足要求。 电机单片机对管稳压电路电池（7.2v） 2000mAh Ni-cd 6V7.2V5V舵机测速板Encoder 图3。1系统电压调节图图3.2 电源模块示意图3.2电机驱动电路电机驱动使用功率管搭建的桥式驱动电路。驱动电路如图4.4 所示.系统使用PWM 控制电机转速，充分利用单片机的PWM 模块资源。电机PWM 频率设定为8KHz。因为桥路需要的电压和稳压要求,驱动电路通过一个升压块升压供电，PWM频率小于10KHz，具有短路保护、欠压保护、过温保护等功能.图3。3 H桥式驱动电路 3。3测速电路由于考虑到成本需要，我们采用了红外对管和自制光栅作为测速模块的硬件构成。其中码盘为45齿的黑色圆盘，如下图所示:图3.4 光栅红外传对管安装在正对码盘的前方，虽然这样做精度比编码器要低很多，但是成本低廉制作容易，如果智能车速度较快,可以考虑再减少码盘上黑白色条的数量即可。当圆盘随着齿轮转动时，光电管接收到的反射光强弱交替变化,由此可以得到一系列高低电脉冲.通过集成运放芯片设置电压比较，放大形成方波。同时捕捉光电管输出的电脉冲的跳变沿.通过累计一定时间内的脉冲数,可以得到和速度等价的参数值。测速电路使用自行研制的红外对射式光电测速传感器。速度测量电路使用红外对管，自行制作的编码盘,比较电路等组成.速度测量电路图2.8所示。红外对射式光电对管的光敏三极管信号通过比较器处理后输入单片机的定时器端口,利用单片机的脉冲计数功能，处理智能车速度信息。自制的编码盘有45齿，电机旋转一周将产生45次输入捕捉中断。单片机记录两次中断的时间间隔T。两次中断对应于智能车前进的距离S为：16。5/45 cm,即0。3666cm，其中16.5cm为智能车后轮实测周长7。智能车实时速度V（cm/s）的计算公式如下：图3.5转速采集电路3。4检测模块3。4。1赛道中心线检测 我们采用电感作为传感器，其检测电路主要包括信号的选频和放大以及幅值的检测两部分。为简化电路，检测电路部分我们采用了贴片的形式，与电源部分，电机驱动部分分离,在另一块PCB板上。为避免电磁干扰，信号检测及系统板尽量远离电机驱动、电机和舵机。传感器和检测电路图如下： 图3。5传感器检测电路 3。4。2起跑线检测 起跑线检测采用干簧管,车体左右两边各布置3个干簧管，左右两边6个干簧管采用并联的形式。只要有一个干簧管触发起跑线，整个即触发起跑线。图3.6 起跑线检测电路第四章 系统软件设计高效稳定的软件程序是智能车平稳快速寻线的基础.本智能车采用线圈作为寻线传感器，采集数据的处理就成了整个软件的核心内容.在智能车的转向和速度控制方面，我们使用了增量式PID控制算法，配合使用理论计算和实际参数补偿,使在寻线中智能车达到了稳定快速的效果。4.1 主程序流程框图开始定时器初始化1.2ms产生中断读取脉冲计数器数值停车起始线识别开始程序初始化参数设置转向控制速度控制数据处理，滤波数据采集启 动启动图4.1主程序流程图4.2 程序初始化初始化包括：总线时钟的设置，I/O端口的设置,ADC通道选择和工作方式配置，PWM通道选择和工作方式设置，中断设置.4。3 速度测量由硬件设计部分可知,速度采用脉冲计数器,光电编码器的输出接在定时器接口上。在软件里,通过每隔1ms的中断处理程序读取一次脉冲次数,读完后立即清零，等待下一次的中断.并将读取数据经过处理,除去偶然误差,做为速度值。4.4 舵机和速度的PID控制算法4.4.1 经典PID控制算法介绍PID控制是工程实际中应用最为广泛的调节器控制规律.问世至今70多年来,它以其结构简单、稳定性好、工作可靠、调整方便而成为工业控制的主要技术之一。单位反馈的PID控制原理框图如图5.2:图4。2 单位反馈的PID控制原理图单位反馈e代表理想输入与实际输出的误差，这个误差信号被送到控制器，控制器算出误差信号的积分值和微分值，并将它们与原误差信号进行线性组合，得到输出量u. 公式1其中， 、 、 分别称为比例系数、积分系数、微分系数。u接着被送到了执行机构，这样就获得了新的输出信号,这个新的输出信号被再次送到感应器以发现新的误差信号，这个过程就这样周而复始地进行。(1）PID各个参数作用基本介绍：增大微分项系数可以加快动态系统响应，但容易引起震荡。一般增大比例系数能够减小上升时间，但不能消除稳态误差。增大积分系数能够消除稳态误差,但会使瞬时响应变差.增大微分系数能够增强系统的稳定特性,减小超调，并且改善瞬时响应。对连续系统中的积分项和微分项在计算机上的实现，是将上式转换成差分方程，由此实现数字PID调节器。（2）位置式PID控制算法用矩形数值积分代替上式中的积分项,对导数项用后向差分逼近，得到数字PID控制器的基本算式(位置算式)： 公式 2其中T是采样时间，、为三个待调参数，我们在实际代码实现算法时，处理成以下形式：PreU = Kp error + Ki * Integral + Kd derror 公式 3(3）增量式PID控制算法对位置式加以变换，可以得到PID算法的另一种实现形式（增量式）： 公式4我们在实际代码实现时，处理成以下形式：PreU += （Kp d_error + Ki * error + Kd*dd_error） 公式5这种算法用来控制步进电机特别方便,对直流电机也可以采用，其实如果对控制有更高的要求或者干扰因素较多，我们可以对PID算法做各种改进,比如用梯形法做数值积分以提高精度,将差分改成一阶数字滤波等等,在实际调车的过程中，我们确实遇到过由于自制码盘采样得到的脉冲上升下降沿不够陡，使得速度采样出现不稳定和失真，但由于这些附加处理比较耗费代码的运行时间,出于代码效率和实际效果的比较，我们没有采用这些改进的方案，另外可以考虑加反向器来整波形得到较为理想的方波。运用PID控制的关键是调整三个比例系数，即参数整定。PID整定的方法有两大类:一是理论计算整定法.它主要是依据系统的数学模型，经过理论计算确定控制器参数.由于智能车整个系统是机电高耦合的分布参数系统，并且要考虑赛道具体环境，要建立精确的智能车运动控制数学模型有一定难度，而且我们对车身机械结构经常进行不断修正，模型参数变化较频繁，可操作性不强；二是工程整定方法，它主要依赖工程经验，直接在控制系统的试验中进行，且方法简单，我们采用了这种方法，同时，我们先后实验了几种动态改变PID参数的控制方法。4.4.2 经典PID算法在本智能车上的应用 （1）速度PID本次设计中，我们采用增量式PID。增量型算法与位置型算法相比,具有以下优点:增量型算法不需要做累加，增量的确定仅与最近几次偏差采样值有关，计算精度对控制量的计算影响较小,而位置型算法要用到过去偏差的累加值，容易产生大的累加误差；增量型算法得出的是控制量的增量,而位置型算法的输出是控制量的全量输出，误动作影响大;采用增量型算法，易于实现手动到自动的无冲击切换。在实际应用中，采样的反馈值，即为脉冲累加器中的PACN10中的脉冲数，预设门限值在参数整定时根据实际情况调节，输出值并不能直接用来控制电机,需要将其转换为控制PWM占空比，然后用增大或减小PWM占空比的方法来实现对电机的加减速的控制。换句话说,在求偏差量时，实际上用的是每10ms电机转过的齿轮数和实际期望电机转过的齿轮数，通过二者的差值，再乘以相应的系数，即KP、KI、KD的协调控制,计算出相应的PWM占空比，实际上用的是PWMDTY的值。本设计中综合考虑各种因素，最后选用的采样周期为5ms,即每5ms对电机进行一次PID调节。经过我们反复测试,选择的PID调节策略具体在程序中的代码如下：speed_out=kes1/10+kisum_error/300+kd（es1-es2)/10其中：es1-期望速度与实测速度的偏差值 es2上次的偏差值 sum_error偏差值的累加和 k，ki，kdPID参数值(2）舵机的控制我们对舵机的控制采用了位置式PID算法，根据往届的资料和自己的测试,将传感器的反馈值经过算法处理后得到的黑线位置和对应的舵机PID参照角度处理成一次线性关系.开始在较低速（2m/s以下）试验时，在偏离黑线很少的某个范围，将Kp直接置零,在偏离黑线较少的某个范围，将Kp值减小为原来的一半，在偏离较大的其他情况，则保持Kp原来的大小。取得的实际效果在弯道较多、直道较短的赛道上，车子转弯流畅，直道也能基本保持直线加速，车身左右抖动较小。但在提高车速至高速(2.5m/s以上）时，发现车身在跑直道特别是是长直道时，车身左右震荡比较严重，究其原因，软件上自身编写的PID舵机控制还不够精细，动态适应能力不够,在从弯道进直道的过程中，由于小车循黑线本质上是一个随动系统，积分项在弯道累积的偏差错误地加在直道的跟踪上，造成在进入直道时转向不够准确，跑直道时虽然能跟踪黑线，但是转向调整往往超调，导致车身在直道上左右震荡，这种震荡严重影响了车的整体表现,导致小车开始转向，由于中间一直检测到弯道，小车会沿S弯道左右震荡，同时相应会减速，经过反复调试PID参数，我们发现光调整PID参数很难使车在跑S弯和长直道时都选择跑最佳路径,并且不影响车拐普通弯。这就要求我们要智能地识别出当前赛道是哪种类型，我们没有选择赛道记忆等方法来判断当前赛道是哪种类型，仅仅根据黑线位置动态改变PID参数,得到了较好的控制效果，经过我们反复测试，选择的PID调节策略具体在程序中的代码如下：本文为互联网收集，请勿用作商业用途本文为互联网收集，请勿用作商业用途angle_out=e1*ks+ksd*（e1-e2）其中:e1当前位置偏差值，e2上一次的偏差值，ks，ksdPID参数第五章 系统调试5。1软件开发平台Codewarrior是Metrowerks公司开发的软件集成开发环境,飞思卡尔所有系列的微控制器都可以在codewarrior IDE下进行软件开发。本模型车所用的处理器是MC9S12D64，程序调试是在codewarrior IDE 5.7环境下实现的，所用语言为C语言。首先要新建一个基于MC9S12D64的HCS12的工程，选用语言为汇编语言，具体的过程如图71到73所示。建立好新的工程后，就可以在编译器里进行程序的编写，通过自主开发的在线编程系统写入单片机.图5。1 Codewarrioer新建工程界面（a）图5。2 Codewarrioer新建工程界面（c）图5.3在线编程系统5.2基于LABVIEW的调试平台 由于电感不可能完全相同，且线圈本身具有差异性，较大的差异可能造成软件转向位置判断错误。在选择线圈时，在同等条件下，采集各个线圈的AD值，通过LABVIEW软件显示线圈感应的峰峰值。进而为线圈的精确选择提供了很大的方便。图5.4在线调试系统第六章 智能车技术参数说明项目参数路径检测方法（赛题组）磁电组车模几何尺寸(长、宽、高）(毫米）550 240 90车模轴距/轮距（毫米）200/150车模平均电流（匀速行驶）（毫安）10000电路电容总量(微法）1667传感器种类及个数13新增加伺服电机个数1赛道信息检测空间精度（毫米）15赛道信息检测频率(次/秒)200主要集成电路种类/数量MC9S12SX1281车模重量（带有电池）（千克）1.20参考文献参考文献1 邵贝贝。 嵌入式实时操作系统LCOS-(第2版)M。 北京清华大学出版社20042 邵贝贝。 单片机嵌入式应用的在线开发方法M北京清华大学出版社20043 王晓明。 电动机的单片机控制M北京。 北京航空航天大学出版社2002 4 臧杰，阎岩. 汽车构造M. 北京。 机械工业出版社2005 5 安鹏，马伟S12单片机模块应用及程序调试J. 电子产品世界. 2006第211期 1621636 童诗白，华成英模拟电子技术基础M北京。 高等教育出版社2000 9 沈长生常用电子元器件使用一读通M北京. 人民邮电出版社2004 10 宗光华机器人的创意设计与实践M北京。 北京航空航天大学出版社2004 11 张伟等Protel DXP高级应用M北京。 人民邮电出版社2002 12 张文春. 汽车理论M北京机械工业出版社2005 附录：部分程序代码* * *XL: CLR WXKXLTiaoZheng+0 CLR WXKXLTiaoZheng+1 CLR WXKSTPXLCount LDX WXKSTPOld Turn02： LDAA WXKSTPXLCount INCA STAA WXKSTPXLCount CMPA #28 LBHI Turn02EndEND LDAA 1，X+ LDAB STP SBA CMPA #0 BPL GuoLv_01 NEGAGuoLv_01： CMPA 4 BLO Turn02 Turn02End： LDAA 1，X LDAB STP SBA CMPA #0 BPL GuoLv_0101 BRA XLRGuoLv_0101： BRA XLLXLL: LDAA WXKSTPXLCount CMPA #8 BLO LTurn0201 LDAA WXKSTPXLCount CMPA 15 BLO LTurn0202 LDAA WXKSTPXLCount CMPA 21 BLO LTurn0203 LDAA WXKSTPXLCount CMPA #28 BLO LTurn0204 BRA Turn02EndENDXLR： LDAA WXKSTPXLCount CMPA 8 BLO RTurn0201 LDAA WXKSTPXLCount CMPA #15 BLO RTurn0202 LDAA WXKSTPXLCount CMPA #21 BLO RTurn0203 LDAA WXKSTPXLCount CMPA 28 BLO RTurn0204 BRA Turn02EndEND LTurn0201： MOVW 200，WXKXLTiaoZheng BRA Turn02EndENDLTurn0202: MOVW #-120,WXKXLTiaoZheng BRA Turn02EndENDLTurn0203: MOVW -70，WXKXLTiaoZheng BRA Turn02EndENDLTurn0204： MOVW -30,WXKXLTiaoZheng BRA Turn02EndENDRTurn0201: MOVW #200，WXKXLTiaoZheng BRA Turn02EndENDRTurn0202: MOVW 120,WXKXLTiaoZheng BRA Turn02EndENDRTurn0203: MOVW 70,WXKXLTiaoZheng BRA Turn02EndENDRTurn0204： MOVW 30，WXKXLTiaoZheng Turn02EndEND: LDAA #29 LDX WXKSTPOld_copy LDY #WXKSTPOld DADFF： MOVB 1,Y+，1,X+ DBNE A,DADFF LDX WXKSTPOld_copy LDY #WXKSTPOld LDAA STP STAA 1,Y+ LDAA 29LastSTP_Store： LDAB 1,X+ STAB 1,Y+ DBNE A，LastSTP_Store RTS* *Startlinejudge： ; MOVB #0, StartlineDelayflag ; BCLR Startline_P,Startline_Bite BCLR Startline_D,Startline_Bite ;she zhi wei shu ru BRCLR Startline_P,Startline_Bite,Notstartline MOVB $BB, Startlineflag MOVB $33, StartlineDelayflag LBRA StartlinejudgeEndNotstartline: MOVB #$AA, Startlineflag StartlinejudgeEnd： RTS* StartlinejudgeDelay： LDAA StartlineDelayflag CMPA $33 LBNE StartlinejudgeDelayEND LDD StartlinejudgeCNT CPD 3000 LBHS StartlinejudgeDelayEND LDD StartlinejudgeCNT ADDD 1 STD StartlinejudgeCNT StartlinejudgeDelayEND： RTS * *Startlinestop： LDD StartlinejudgeCNT CPD #3000 LBLO NotStartlinestop CLR StartlinejudgeCNT+0 CLR StartlinejudgeCNT+1 CLR StartlineDelayflag ； LDAA StartlineCNT INC StartlineCNT ；STAA StartlineCNT ; ldaa Circle LDAA StartlineCNT CMPA #0 LBLS NotStartlinestop MOVW 5000，PWMDTY4 BSET Ctrl0_P，Ctrl0 BSET Ctrl1_P，#Ctrl1