大连理工大学创新一队技术报告

1、第二届“飞思卡尔”杯全国大学生智能车竞赛技术报告学 校：大连理工大学队伍名称：创新一队参赛队员：姚明江 王超 赵亮带队教师：吴振宇 I关于技术报告和研究论文使用授权的说明 本人完全了解第二届“飞思卡尔”杯全国大学生智能车竞赛关于保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人，比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。参赛队员签名： 带队教师签名： 日 期： 目 录第一章 引言1第二章 小车模型的改造和安装3第三章 各模块电路的方案选择和具体设计53.1中央控制

2、电路的设计53.2主电机驱动电路的选择和设计73.3速度检测元件的选择和电路设计93.4 赛道检测传感器的选择和电路设计103.5 系统供电设计113.5.1 主控制板的供电123.5.2 各传感器的供电123.5.3 电机的供电123.5.4 舵机的供电123.6 人机接口模块的设计13第四章 控制软件的设计154.1 驱动层软件设计154.1.1.系统时钟设置154.1.2. 主电机和舵机驱动154.1.3 速度检测的计数和处理164.1.4 赛道检测的数据采集174.2 决策层软件算法设计174.2.1 滤噪与图像信息提取184.2.2 舵机转角的决策184.2.3 速度闭环控制19第五

3、章 调试说明205.1编译软件使用205.2硬件电路设计调试205.3软件算法效率的测试205.4机械结构调试20第六章 小车模型改造后的主要技术参数22第七章 结论24参考文献I附录A 迟滞比较器门限电压估算公式的推导III附录B 系统源代码V第一章 引言本设计采用MC9S12DG128单片机作为智能小车的检测和控制核心。测速装置使用光电编码器对电机转速进行检测，通过对速度的检测，整个系统闭环控速，系统稳定性增强。电机控制模块使用集成的单片电动机控制芯片MC33886，主控制器按预定的运行规则和程序，发控制信号给电机控制芯片，对主电机转速进行调节，从而控制小车的加速和制动。赛道检测模块采用c

4、cd传感器，通过算法处理，将赛道的情况和转弯的趋势较精确地计算出来，通过主控制器将转角信号送给舵机，实现小车的巡线功能。本设计还在大赛规则允许的范围能对车模的机械结构作了一定的改进，对改善小车的整体性能有极大的帮助。本设计方案的主要特色有以下几方面：为使主控制器的电路达到最简，单独设计了主控制器的最小系统，简化了外围电路的设计，使主控系统达到最优。采用光电编码器对电机进行转速检测，由此经过计算得到车体速度，保证速度测量的准确性，其测量值可以满足对速度控制的要求，且不易受外界干扰。采用了ccd传感器对赛道检测，可以预测较远距离的赛道情况，并采用可靠的算法对检测所得的数据进行处理，并做出决策，由此

5、对舵机、电机进行控制，实现了较精确的转向控制和快速的行驶。在规则允许范围内改进了小车的机械结构，提升了小车的整体性能。整个系统的系统构成如图1.1。第二章 小车模型的改造和安装根据比赛要求，舵机本身、主电机和电池部分未作任何改动，电池仍然固定在原位置，小车的改造主要侧重于舵机的安装位置、连接件和ccd传感器与电路板的固定。基于小车重心的考虑，中央控制电路板固定在小车中间靠前的位置，这样还可以避免主电机转动的干扰。ccd传感器的位置固定在前轮转动轴后不远处，旨在减小ccd传感器支架在小车转向时的转动惯量，使其不至于在转向时摇晃。速度检测传感器（光电编码器）固定在电机旁，通过齿轮与传动轴的齿轮咬合

6、，以便将转速转换成脉冲信号。（具体原理见第三章）舵机的安装位置垂直于车体，以达到两根转向轴长度相等的目的。并自制了舵机的连接件，增长了连接件的长度，以提高舵机的响应速度。第三章 各模块电路的方案选择和具体设计3.1中央控制电路的设计中央控制电路是整个系统的核心，控制器完成主电机控制、寻线响应、舵机转动控制和速度获取等功能。其中对主电机和舵机的控制是很重要的任务。通常采用PWM信号来调节电机转速和舵机转角。Freescale公司推出的MC9S12DG128单片机具有PWM输出功能，而且其片上的PWM功能具有独立的定时计数模块，极大地简化了硬件系统的设计。MC9S12DG128单片机有QFP-80

7、和LQFP-112两种封装，考虑到系统可能会比较复杂，因此决定选用资源相对较丰富的LQFP-112封装的单片机作为主控制器。在电路设计上，采取了模块化的设计思想，整个电路由最小系统板和扩展板构成。最小系统板囊括了MC9S12DG128单片机系统的必要组成部分，包括时钟电路、锁相环滤波电路、A/D转换器滤波电路、调试用的LED、电源以及各I/O口接口和BDM调试器接口等。扩展板上规划了小车的各个设备的接口电路，包括电机驱动电路、各I/O口的引出线、舵机接口、人机交互电路、测速电路接口以及赛道识别传感器接口。最小系统板和扩展板的原理图如图3.1和图3.2。第三章 各模块电路的方案选择和具体设计图3

8、.1 单片机最小系统原理图图3.2 扩展板原理图单片机端口资源的分配情况如下：MC9S12DG128单片机有A、B、E、H、K、J、M、P、S、T等数字I/O口，还具有两个A/D输入端口。在本作品中用PTS口和A/D口采集赛道检测传感器送出的数字信号和模拟电压信号，PTT端口完成对速度检测传感器信号的计数功能，PTH口作为拨码开关量的输入，PTP端口完成对主电机转速、方向的控制和舵机的驱动，PTM口作为12864LCD的数据口。3.2主电机驱动电路的选择和设计主电机为永磁式微型直流电动机。它的驱动方案主要有三种：电磁继电器开关控制、达林顿管或MOS管组成的H型电路控制和单片电动机控制芯片控制。

9、第一种和第三种方案电路较简单。但是继电器响应速度慢，易损坏，可靠性不高，而且体积大，显然不适合小车的使用。第二种方案效率高，而且控制灵活，但其外围电路复杂，体积和重量都不能满足要求。如果采用第三种方案，就需要解决散热和驱动电流问题。考虑到小车的主驱动电机为微型永磁式直流电动机，工作电流相对较小，而且小车的设计有重量和体积的限制，所以决定采用第三种方案。电机驱动芯片选用Freescale公司的MC33886。该芯片的内部结构与大功率管构成的H桥式电路基本相同，只是在此之上加入了逻辑控制电路和过流过热保护电路。具体的电路设计如图3.3。图3.3 电机驱动电路MC33886是Freescale公司推

10、出的H桥集成电路，最大电流可以达到5A。其内部结构如图3.41。其中D1、D2为制动端，当输入有效信号的时候，输出端口立即显现三态，电动机迅速制动。FS为错误标志段，当出现错误时，该端显高电平。IN1和IN2为两个半桥的信号输入端，OUT1和OUT2为两个半桥的输出端。在设计时，将IN1端接地，IN2端接到单片机的PWM5信号端，通过控制信号的占空比来调整OUT1和OUT2两端的电压，实现电机速度调整。图3.4 MC33886内部结构3.3速度检测元件的选择和电路设计速度的检测传感器可以使用红外传感器、霍尔元件或光电编码器。考虑到红外传感器与霍尔元件的方法不是太稳定，且需要辅助电路，因此选择了

11、光电编码器进行速度检测，对电机输出的转速进行检测。光电编码器通过齿轮咬合在主电机齿轮上，当主电机转动时，光电编码器会产生脉冲信号。通过检测脉冲信号的频率或周期，就可以得到一个与小车速度成正比的数值，这个数值经过量纲换算，就可以得到小车的速度。小车后轴的变速齿轮用于将主电机的高转速变成驱动轴的低转速。在齿轮的边缘位置夹放一个红外槽形光耦，检测光路就放在齿轮的齿上。当光路上是齿尖时，发射管发出的红外信号被齿尖遮挡，接收管不能接收到信号，显高电平。当检测点之间是齿槽时，发射管的信号被接收管接收，产生一个负阶跃脉冲信号，经过两个施密特触发器整形后送给单片机，触发单片机计数响应。3.4 赛道检测传感器的

12、选择和电路设计赛道检测传感器，我们选用了面阵ccd，它的输出信号是PAL 制式的复合全电视信号，每秒输出50帧(分为奇场和偶场)。当ccd采集图像时，奇场和偶场不是同时采集的，因此，可以在每场信号都对路径进行识别。由于单片机集成AD转换器转换速度的限制，采集到的图像分辨率较低，需要对单片机及AD转换器超频才能满足需求，会影响到系统的稳定性，因此本设计直接将ccd采集的信号送入单门限比较器进行比较，再直接将比较后的二值化信号送入单片机进行处理。这种方法既减少了对存储空间的需求，减少了运算复杂度，又可以得到比较高的检测精度。ccd输出的视频信号分为两路，一路经同相比例放大后进入LM393 比较器，

13、比较阈值电压通过电位器进行调节。另一路送入LM1881进行视频信号同步分离，可得到复合同步信号、场同步信号、奇偶场信号等。视频信号的放大及二值化电路如图3.6。图3.6 视频放大及二值化电路3.5 系统供电设计考虑到小车的动力主要来源于电池，必须采用集中供电的办法，而控制板、舵机和主电机的供电又不能采用同一电压值的电源，因此设计了电源电路，为系统各模块供电。电源电路如图3.7。图3.7 电源电路3.5.1 主控制板的供电主控制板的电流较小，直接从+5V电源取电。为了避免短路和过压，在主控制板上设计了过压过流保护电路。主控制板上还引出了若干路电源，供电流不大的外围电路取电。3.5.2 各传感器的

14、供电测速传感器的电流不大，供电较灵活，可以从主控制板上取电。而赛道检测传感器电路的电流比较大，所以传感器电路不从主控制板取电而直接从+5V电源取电，以避免电流大时主控板保护电路误动作，造成电压损失。3.5.3 电机的供电为了使电机达到理想的转速，获得理想的转矩，电机直接从+7.2V电池取电。因为主电机驱动芯片中集成了电机电源和逻辑电源的隔离电路，因此电机转动时对其它电源的干扰大大降低了。3.5.4 舵机的供电舵机的响应速度与其电源电压有关。因此，为了获得更快的响应速度，舵机的供电采用其工作上限电压+6V，用LM7806将电池电压稳压成+6V后为其单独供电。3.6 人机接口模块的设计为了调试时监

15、控小车的各项参数，同时提高小车对赛道的适应性，特设计了人机接口电路。输入部分采用拨码开关和发光二极管，输出部分采用了带汉字字库的12864点阵的LCD显示模块。调试的过程中，可以显示主要参数，监控小车的运行。同时，可以通过拨码开关对程序状态进行设置，使小车根据不同的情况采取不同的决策方法，使调试更加具有针对性。本模块电路如图3.8。图3.8 人机接口电路设计第四章 控制软件的设计4.1 驱动层软件设计4.1.1.系统时钟设置为了加快系统响应速度，使用单片机内部的锁相环电路将外部的16MHz时钟倍频到48MHz，总线时钟24MHz，以提高运算速度和系统响应速度。具体的设置如下：REFDV = 0

16、x01; SYNR = 0x02; /设置系统时钟为48MHz靠锁相环产生的时钟频率由下面的公式得到： 公式3显然，采用16MHz的外部晶体振荡器，可以得到48MHz的系统频率、24MHz的总线频率4。4.1.2. 主电机和舵机驱动考虑舵机对PWM信号的要求较高，为了提高精度，将单片机的8路8位的PWM输出调整为4路16位的PWM输出。主电机驱动采用PWM输出控制，利用MC9S12DG128的PWM输出，将调制信号送到MC33886的IN1信号输入端，通过调整占空比调整电机转速，IN2输入端接地。舵机的控制同样采用PWM输出控制，舵机的转角是通过调整高电平的宽度来控制的。根据要求，S3010型

17、舵机的PWM周期为18ms20ms，两侧的极限位置的高电平宽度为1ms和2ms。在控制的过程当中，根据小车前轮的实际转角，对上面的值稍做了些补偿。对PWM模块的具体设置如下：第四章 控制软件的设计 PWMPOL = 0xff;/设置极性为高电平 PWMCLK = 0;/选择时钟为 Clock A 和 B PWMCTL = 0xf0; PWMPRCLK = 0x31;/Clock A= Bus Clock/2 ; /Clock B= Bus Clock/8 PWMPER23 = PERIOD23;/PERIOD23为23通道的周期 PWMPER45 = PERIOD45;/PERIOD45为45

18、通道的周期 PWMDTY23 = 0; PWMDTY45 = 0;4.1.3 速度检测的计数和处理速度检测电路的输出是频率变化的矩形波。要测量小车的速度，主要方法有两种：测周法和测频法。测频方案是使用增强型定时器溢出中断功能产生一个43ms的定时中断，在这个设定时间内，用单片机的脉冲累加器对红外传感器产生的脉冲进行计数，定时溢出时，记录红外传感器产生的脉冲数。因为电机的转速和红外传感器产生的矩形波频率成正比，通过测频，就可以间接地得到电动机的转速，进而得到小车的整体速度，对小车采取闭环控制。测周方案是使用单片机的实时中断，设定一个47us的中断，同时启用输入捕捉功能，对测速传感器传回的脉冲信号

19、的上升沿进行捕捉。使用实时中断对上升沿信号之间的时间间隔进行计数，以T=N47（N为记得的实时中断数量）来计算电机转速。测周的响应周期由电机的转速决定，一般不超过10ms，所以响应速度大大提高。由于所采用的光电编码器每圈能输出200个脉冲，数量较多，综合考虑了车速以及控制精度这些因素之后，我们选择测频方案，完全满足检测速度的需要。4.1.4 赛道检测的数据采集ccd传感器采集赛道数据流程，如图4.1。ccd1881行场同步分离单门限比较器行同步场同步PE3IRQPJ0图4.1 ccd采集流程图4.2 决策层软件算法设计系统软件流程图，如图4.2。主程序初始化ccd采集分析数据、计算角度速度控制

20、图4.2 主程序流程图4.2.1 滤噪与图像信息提取一场图像采集完成后，得到一幅20*20的原始二值图像信息，如图4.3（A）所示，同时给标志变量VideoFinFlag置1。主程序中检测到此标志的时候，启动滤噪程序。考虑到图像中有效的信息为简单的黑色线条，必然具有很强的连续性。所以，可以采用从图像一侧到另一侧，逐行邻域查询的办法。具体来说，就是根据已经确定的前行黑线中心位置，在本行的一个邻域内查找黑点。超出这个邻域的黑点都认为是无效的。这样，只要能够确定某一行黑线的中心位置，就能逐行确定各行黑线的中心位置，从而，也就提取出了图像中最基本的信息黑线中心位置轨迹（如图4.3（B）所示）。原始图像

21、信息 处理后的信息00000000000010000000 200000000000010000000200000000000100000000100000000000100000000100000000001000000000000011000001000000000000010000001000000000000000000001100000000000000000001100000000000000000011000000000-100000000011000000000-1000000000111000000000000000000111000000000000000000111000

22、00000000000000111000000000-100000000111000000000-100000001110000010000-200000001110000000000-200000001110000000000-200000011110000000000-3(A)(B)图4.3 处理前后的图像信息4.2.2 舵机转角的决策由于CCD图像传感器的应用，大大提高了车的前瞻性。经测量，本系统预测距离可达1米以上。但是，考虑到各部件延时严重且具有很大不确定性，所以预测距离越远，控制算法就会越复杂。综合各种因素考虑，我们最终将预测距离定在80100cm。黑线中心位置轨迹中包含着两方面的

23、信息，一方面是反映前方道路趋势的信息，另一方面是反映车体与轨道偏离程度的信息。经过反复试验，我们发现，前者可以通过计算黑线的斜率来很好的描述。而后者，恰恰就是屏幕最下端黑线的位置。最后用这二者的线性组合得到舵机转角。有了这两方面信息，可以使智能车既能充分利用前瞻性，又能不过分地偏离轨道。从而得到了较理想的转角控制策略解决方案。4.2.3 速度闭环控制速度控制采用数字PID算法，通过整定参数，使电动机的响应达到满意的速度和精度。PID控制的基本思想是对误差进行比例积分微分运算，实现对系统的基本控制。离散后的PID用误差的累积加和代替积分，用两次误差的差分代替微分。当采样周期很小时，可以近似地认为

24、系统为连续系统。通过整定三种运算的增益来调整电机系统的响应速度和超调量的大小。第五章 调试说明以下的几个问题是调试中遇到的难点：5.1编译软件使用编译软件使用时受到licence的限制，单一文件不能超过1k。因此采用多个文件联结编译，问题便得到了有效的解决。5.2硬件电路设计调试在调试过程中，采用LED显示下层传感器逻辑状态，以便检查下层数字量传感器的稳定性及方向性；对于上层模拟量传感器使用LCD显示其各个数值，同时显示车轮转速等信息。因为上层传感器的位置较高，容易受到外界光线的干扰。调试时，由于各个红外传感器光学和电学特性的差异，上层传感器有时出现状态跳变的情况，导致小车的自适应性较差。为了

25、增强小车的自适应性，在软件中加入了对小车的训练。通过训练，控制器记录下小车上层各传感器感受到的最大值和最小值，然后对其进行线性补偿。这样就可以把小车上层各传感器的特异性消除，解决各传感器光学和电学特性不一致的问题。加入训练之后，小车的运行效果有了很大改善。5.3软件算法效率的测试为了测试软件的算法效率，考虑采用实时时钟进行计时。但是如果在调试的过程中采用单步或设置断点调试，实时时钟会对调试造成麻烦。在实际的调试中，采用了固定端口取反的办法测试算法效率。在每一次算法循环结束之后，对一个端口（例如PORTA）进行取反操作。随着算法的不断循环，在测试端口上会产生一定频率的方波。用示波器观察方波的半周

26、期，就可以得到算法执行一次的准确时间。实践证明，这种方法非常有效。5.4机械结构调试机械结构的调试，主要问题是舵机、后轮位置的调节。舵机的调整采用机械调整和软件补偿并行的方法。先通过软件发指令给舵机，使其位于正中位置，然后调整两个连杆，目测两轮处于平行位置，然后通过软件对直道的转角进行补偿，使其在直道上基本不出现抖动。需要注意的地方是，舵机的调节与连杆的尺寸有着密切的联系，需要严格按照说明书上的尺寸进行调解，否则会出现前轮的左右轮受力不均，无法对准零点的现象。后轮的调节要简单的多，调节电机和后轮转轴，使其能够自由转动且不发生左右摆动即可。第六章 小车模型改造后的主要技术参数小车模型改造后的主要

27、技术参数见表6.1和表6.2。表6.1 小车外形参数表l/cmw/cmh/cmm/g2817401330 l：长 w：宽 h：高 m：质量表6.2 小车电路参数表P/WNs/cmT/msCs/F5.622.142717P：电路功耗 N：传感器个数 s：赛道信息检测精度 T：代码运行周期 Cs：电容总容量第七章 结论通过这次设计，小车基本实现了按黑线规定的赛道较高速行进，能较好的完成各项技术指标。文章都对其所涉及方案论证、系统设计，以及单片机程序代码设计以及软件流程进行了较详细的阐述。值得一提的是，在传感器的布局和上层红外传感器检测的算法设计上进行了创新性的设计，使其大大增加了车体的前瞻性和对赛

28、道的检测精度。但系统在整体控制上还可以再进一步优化，希望在接下来的时间内得以完善。参考文献1 Freescale Semiconductor. MC33886.pdf, 2006, /files/analog/doc/data_sheet/MC33886.pdf2 卓晴, 王琎, 王磊. 基于面阵CCD的赛道参数检测方法J. 电子产品世界, 2006，13(4)：141-143.3 康华光. 电子技术基础模拟部分（第四版）M. 北京: 高等教育出版社, 19994 邵贝贝. 单片机嵌入式应用的在线开发方法M. 北京: 清华大学出版社, 2004附录

29、A 迟滞比较器门限电压估算公式的推导图A-1 反相输入迟滞比较器电路一般情况下，输出电压vO与输入电压vI不成线性关系，输出电压转换的临界条件是vPvD=vI，显然这里的vP就是门限电压。根据图A-1，利用叠加原理有： 公式A-1根据输出电压vO的不同值（VOH或VOL），可分别求出上门限电压VT+和下门限电压VT-分别为： 公式A-2 公式A-3那么，门限宽度为： 公式A-4XXIII附录B 系统源代码/摄像头预测最近距离11cm/#include /* common defines and macros */#include /* derivative information */#inc

30、lude IO.h#include 12864.h#include PWM.h#includelzhao.h/#include mathf.c/舵机0度角 PWMDTY23=4140#pragma LINK_INFO DERIVATIVE mc9s12dg128b/全数字视频采集1.0/CPU总线时钟48MHz/采集分辨率46*25/采集范围中间280行，每8行一采/T值计算公式:T=t(s)*3000000/#define horizon stack0-stack27#define GF PORTAB_BIT15=1#define GB PORTAB_BIT15=0int stack28,s

31、ecure=0,n,horizon;uchar Image4628,DataBuffer2827;uchar line,S_Flag;uchar VideoFinFlag,VideoLineCount,VideoDotCount;uchar VSS; /Video Sample Statusint T;unsigned int startcol,endcol;unsigned char ScreenFlag,flag;float xxx;float k,s,sec;int count;unsigned int speed_chable,speed_on;/-PID-/unsigned int

32、speed_m,speed_e,speed_e0;int ek,dek,ek_old,dek_old;unsigned char speed_flag = 0;/*/void cal(void);void MotoControl(void);/*/float at(int x,int n0) float y; y=46*x/n0+2 ; return y; void compute(void) s=stack26; xxx=stackendcol+1-stackstartcol; k=xxx/n*46+2; void main(void)DisableInterrupts; while(PTH

33、_PTH7); count=0;PLL_Init();Start_PLL();lcd_initial();Video_Init();PWM_Init();DDRA=0xff; DDRH_DDRH2=1; MDCInit(); PaiInit(); RealTimer_Int(); DDRA=0xff; DDRH_DDRH2=1; PTH_PTH2=1; DDRH_DDRH3=1; PTH_PTH3=1; ek = 0; ek_old = 0; speed_chable=1; speed_on=1; GF; speed_e=90; EnableInterrupts;INTCR_IRQEN=1;P

34、WME=0xff;PWMDTY23=4140; PWMDTY45=7000; for(;)if(VideoFinFlag)VideoFinFlag=0; fn();cal();if(speed_chable=0) if(speed_m40) speed_chable=1; speed_on=1; MotoControl(); /* switch (PTH&0xf0) case 0xe0: /PWMDTY23+=10; Delay_12864(5000); Delay_12864(50000); GF;PWMDTY45=1000; break; case 0xd0: /PWMDTY23-=10;

35、 Delay_12864(50000); Delay_12864(5000); GF;PWMDTY45=8000; break; case 0xb0: D2=0; PWMDTY23+; Delay_12864(50000); Delay_12864(5000); break; case 0x70: PWMDTY23-; Delay_12864(50000); Delay_12864(5000); break; LCD_Write_string(line4,PWMDTY:); Disp4(PWMDTY23); Delay_12864(1000); */void interrupt 6 IRQ_I

36、SR()uchar i,j;if(VSS=STOP_SAMPLE)INTCR_IRQEN=0;/关闭IRQ中断PIFJ=0XFF;PIEJ =0x02; /开场同步中断return;elseif(VideoLineCount=ILL) /ILL:采集行数 INTCR_IRQEN=0;/关闭IRQ中断VSS=STOP_SAMPLE;VideoLineCount=0;VideoFinFlag=1;for(i=4;i32;i+)for(j=0;j27;j+)DataBufferi-4j=Imageij;INTCR_IRQEN=1; elseif(VideoLineCount%8)=0) /采集0,6

37、,12.270共46行 INTCR_IRQEN=0;/关闭IRQ中断 VideoDotCount=0; T=IIDLT; /31 = 10.4us /MDCInit(); MCCTL_MCEN=1; MCCNT = T; MCCTL = MCCTL|0X08 ; /把模数常数寄存器的值加载到模数计数器 else VideoLineCount+; void interrupt 24 PortJ_ISR(void)PIEJ &=0xFD;PIFJ=0XFF;T=IILLT; /3600 = 1.20msMCCTL_MCEN=1;MCCNT = T;MCCTL = MCCTL|0X08; /MDCI

38、nit();void interrupt 26 MDC_ISR(void)MCCTL_MCEN=0; /模数计数器关闭MCFLG = 0x80; /清标志位switch (T)case IILLT: /1.1msVSS=WAIT_TO_SAMPLE; break;case IIDLT:/70usline=VideoLineCount/8;for(VideoDotCount=0;VideoDotCount28;VideoDotCount+) INTREVAL;ImagelineVideoDotCount=PORTE_BIT3;VSS=WAIT_TO_SAMPLE;VideoLineCount+;

39、break; /MCCTL_MCEN=1;INTCR_IRQEN=1;/IRQ使能/void cal(void) int xk,xs; /GF; switch(ScreenFlag) case FULLSCREEN: if(abs_s(stackstartcol-stackendcol)5) /直道 xk=1; /xk=0; xs=8; /15 speed_e0=100; else xk=12.5; /10 xs=15; /15 speed_e0=90; /65 /增大转角 sec=0; break;/ case UPPERSCREEN: speed_e0=0; xs=16; / xk=7;

40、xk=7; sec=0; break;/ case LOWERSCREEN: speed_e0=0; xs=25; / xk=22.5; xk=22.5; sec=0; break;/ case ERROR: speed_e0=0; xs=0; xk=0; /sec=secure*38; sec=secure*40; break; default: break; k=at(stackendcol+1-stackstartcol),n-1)*xk; s= stackstartcol*xs; PWMDTY23=4140+k+s+sec; if(speed_chable) speed_e=speed

41、_e0; if(speed_e=0) speed_chable=0; /* if(abs_s(stack0-stack10)-abs_s(stack15-stack27)3) /if (abs_s(stack0-stack10)-abs_s(stack15-stack27)2) /PTH_PTH2=0; S_Flag=0; speed_e=0; /PTH_PTH2=1; */ /void MotoControl(void) / speed_e是给定 speed_m是测定。 / u 和 PWMDTY1 是力信号 int u = 0; ek_old = ek; /将e(k)保存 dek_old =

42、 dek; /将derta(e)保存 ek = speed_e - speed_m; / g_nE 相当于 e(k) dek = ek - ek_old; / dg_nE 相当于 e(k)-e(k-1)即derta(e) if(ek 12) /棒棒控制模块，不知道转入PID 控制没给初值是否行的通 这里应该用增量式PID /期望得到的速度与当前速度的差 u=99; speed_flag = 1; else if (ek -8) /空转时，很可能出现ek= 0) GF; if (u 99) u = 99; else GB; u=30; / PWMDTY45=u*100; PWMDTY45=u*1

43、00; /92 #include /* common defines and macros */#include /* derivative information */#include IO.h#include12864.hextern int stack28,stack128,secure,n,horizon;extern uchar DataBuffer2827;extern uchar VideoFinFlag,VideoLineCount,VideoDotCount;extern uchar VSS; /Video Sample Statusextern int T;extern unsigned int startcol,endcol;extern unsigned char ScreenFlag;const int array2827= -21,-19,-18,-16,-14,-13,-11,-10,-8,-6,-5,-3,-2,0,2,3,5,6,8,10,11,13,14,16,18,19,21,-20,-19,-17,-16,-14,-13,-11,-9,-8,-6,-5,