湖北汽车工业学院电磁组惠捷二号技术报告.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
第五章 系统调试 19
5。1基于LABVIEW的调试平台 19
第六章 智能车技术参数说明 23
参考文献 24
部分程序代码 25

IV
第一章 引言
本智能小车以飞思卡尔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°。
对于模型车,通过调整前桥的螺杆的长度可以改变主销内倾角的大小,由于过大的内倾角也会增大转向阻力，增加轮胎磨损,所以在调整时可以近似调整为0°～3°左右，不宜太大。
主销内倾和主销后倾都有使汽车转向自动回正，保持直线行驶的功能。不同之处是主销内倾的回正与车速无关,主销后倾的回正与车速有关，因此高速时主销后倾的回正作用大，低速时主销内倾的回正作用大.
2。3。3车轮外倾角
前轮外倾角是指通过车轮中心的汽车横向平面与车轮平面的交线与地面垂线之间的夹角［2］，对汽车的转向性能有直接影响,它的作用是提高前轮的转向安全性和转向操纵的轻便性[1］。在汽车的横向平面内，轮胎呈“八”字型时称为“负外倾"，而呈现“V”字形张开时称为正外倾。如果车轮垂直地面一旦满载就易产生变形,可能引起车轮上部向内倾侧，导致车轮联接件损坏。所以事先将车轮校偏一个正外倾角度,一般这个角度约在1°左右，以减少承载轴承负荷，增加零件使用寿命，提高汽车的安全性能。
模型车提供了专门的外倾角调整配件，近似调节其外倾角。由于竞赛中模型主要用于竞速，所以要求尽量减轻重量,其底盘和前桥上承受的载荷不大，所以外倾角调整为0°即可，并且要与前轮前束匹配。
2。3。4 前轮前束
所谓前束是指两轮之间的后距离数值与前距离数值之差,也指前轮中心线与纵向中心线的夹角[2］。前轮前束的作用是保证汽车的行驶性能，减少轮胎的磨损.前轮在滚动时，其惯性力自然将轮胎向内偏斜，如果前束适当,轮胎滚动时的偏斜方向就会抵消，轮胎内外侧磨损的现象会减少.像内八字那样前端小后端大的称为“前束”，反之则称为“后束"或“负前束”。在实际的汽车中,一般前束为0~12mm 。
在模型车中，前轮前束是通过调整伺服电机带动的左右横拉杆实现的。主销在垂直方向的位置确定后，改变左右横拉杆的长度即可以改变前轮前束的大小。在实际的调整过程中，我们发现较小的前束，约束0~2mm可以减小转向阻力,使模型车转向更为轻便，但实际效果不是十分明显。
虽然模型车的主销后倾角、主销内倾角、车轮外倾角和前束等均可以调整，但是由于车模加工和制造精度的问题，在通用的规律中还存在着不少的偶然性，一切是实际调整的效果为准。
2。4其它机械结构的调整
1为有效地防止由于轮胎与轮辋错位而引起的驱动力损失,我们对前轮进行了粘胎处理.
2为了防止小车在高速时急转弯时发生侧滑甩尾等现象，在底盘下部车尾与车身也进行了粘接处理。
3为提高小车的稳定性，我们尽量做到使小车重心降低且使小车的重心后移,如将电池后移.

第三章 系统硬件设计
3。1电源部分
为了能使智能车系统能正常工作，就需要对电池电压调节。其中，单片机系统、车速传感器电路需要5V电压，路径识别的电路电压工作为5V、伺服电机工作电压范围4.8V到6V（或直接由电池提供），直流电机可以使用7.2V 2000mAh Ni-cd蓄电池直接供电。考虑到由于驱动电机引起的电压瞬间下降的现象，因此采用低压降的三端稳压器成为必然。我们在采用LM7806,和LM2940作为稳压芯片。经试验电压纹波小，完全可以满足要求。

电机
单片机
对管

稳压电路
电池（7.2v） 2000mAh Ni-cd

6V

7.2V
5V

舵机
测速板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=k＊es1/10+ki＊sum_error/300+kd＊（es1-es2)/10
其中：es1-期望速度与实测速度的偏差值
es2—上次的偏差值
sum_error—偏差值的累加和
k，ki，kd—PID参数值
(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，ksd—PID参数

第五章 系统调试
5。1软件开发平台
Codewarrior是Metrowerks公司开发的软件集成开发环境,飞思卡尔所有系列的微控制器都可以在codewarrior IDE下进行软件开发。本模型车所用的处理器是MC9S12D64，程序调试是在codewarrior IDE 5.7环境下实现的，所用语言为C语言。
首先要新建一个基于MC9S12D64的HCS12的工程，选用语言为汇编语言，具体的过程如图7—1到7—3所示。建立好新的工程后，就可以在编译器里进行程序的编写，通过自主开发的在线编程系统写入单片机.

图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
主要集成电路种类/数量
MC9S12SX128＊1
车模重量（带有电池）（千克）
1.20

参考文献
参考文献
［1] 邵贝贝。 嵌入式实时操作系统[LC／OS-Ⅱ(第2版)［M]。 北京．清华大学出版社．2004
[2］ 邵贝贝。 单片机嵌入式应用的在线开发方法［M]．北京．清华大学出版社．2004
［3］ 王晓明。 电动机的单片机控制［M］．北京。 北京航空航天大学出版社．2002
[4］ 臧杰，阎岩. 汽车构造［M]. 北京。 机械工业出版社．2005
[5] 安鹏，马伟．S12单片机模块应用及程序调试［J］. 电子产品世界. 2006．第211期． 162—163
[6］ 童诗白，华成英．模拟电子技术基础［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
NEGA
GuoLv_01：
CMPA ＃4
BLO Turn02
Turn02End：
LDAA 1，—X
LDAB STP
SBA
CMPA #0

BPL GuoLv_0101
BRA XLR
GuoLv_0101：
BRA XLL
XLL:
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 Turn02EndEND
XLR：
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 Turn02EndEND
LTurn0202:
MOVW #-120,WXKXLTiaoZheng
BRA Turn02EndEND
LTurn0203:
MOVW ＃-70，WXKXLTiaoZheng
BRA Turn02EndEND
LTurn0204：
MOVW ＃-30,WXKXLTiaoZheng
BRA Turn02EndEND
RTurn0201:
MOVW #200，WXKXLTiaoZheng
BRA Turn02EndEND
RTurn0202:
MOVW ＃120,WXKXLTiaoZheng
BRA Turn02EndEND
RTurn0203:
MOVW ＃70,WXKXLTiaoZheng
BRA Turn02EndEND
RTurn0204：
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 ＃29
LastSTP_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 StartlinejudgeEnd

Notstartline:
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