基于线性CCD的自主行驶小车系统的设计 毕业设计

摘要

本文主要设计制作一辆基于线性CCD自主循迹行驶的智能车,采用飞思卡尔半导体公司生产的32位单片机MK60DN512VLQ10作为核心处理器，主要通过TSL1401线性CCD采集赛道的一维图像信息，反馈给单片机控制舵机转向，电机调速，使小车按照事先架设好的跑道自主行驶，并且能通过ST188红外对管识别出终点（跟边线垂直的黑线段）自动停车。硬件系统由单片机模块，TSL1401线性CCD模块、电源管理模块、电机驱动模块、OLED人机交互模块、光电对管模块以及电机、舵机和编码器组成。软件系统主要由IAR进行开发，控制算法主要采用PID控制。

关键词

智能车；线性CCD；PID

Abstract

This paper mainly introduces the design about a smart car with automatic tracking function based on linear CCD.The control system uses the Freescale Semiconductor's 32-bit microcontroller MK60DN512VLQ10 as the core processor.The TSL1401 linear CCD acquires one-dimensional image information, which is feedbacked to The MCU.Then the MCU controls servo’s direction, motor’s speed, so the car can automatically drive in accordance with beforehand track,identify the end(the black line vertical with sideline) and stop automatically by the ST188. The hardware system includes a microcontroller module,a TSL1401 linear CCD module,a power management module, a motor drive module, two OLED modules, four photoelectric tube modules，a motor, a servo and a encoder. The software system is developed by IAR and the control algorithm mainly uses PID control.

Key words

Smart Car; Liner CCD; PID

1.引言

汽车行业的发展已经超过了100多年，自1980年以来，智能理论控制技术在交通运输工程中已经被越来越广泛的运用，智能汽车的概念在这一背景下也就相应产生和发展[1]。随着科学技术与现代汽车工业的结合发展，两者的产物“智能汽车”能像机器人一样，可以自动地进行环境的感知，规划和决策的运行控制[2]。智能车系统是一个综合多技术领域系统，其包含了自动控制、模式识别、传感技术、电子、电气、计算机、机械与汽车等多个学科专业[3]。因此，对智能车技术的研究能够促进计算机技术、车辆控制理论、摄像头[4]等多传感器信息融合技术等技术的快速发展；可以提高车辆的控制能力和辅助驾驶水平，对优化交通设施，缓解和改善交通问题具有积极的意义；改善工业人员工作环境，提高安全系数，同时还可以提高工业的智能化水平，提高产量。为人类的生产生活带来越来越多的便利，使人类的生活质量越来越高。

智能车最大的要求就是能够自动循迹，像全国“飞思卡尔”杯智能车比赛、瑞萨杯智能车比赛等比赛都是要求智能车能够自动循迹。而光电传感器的选用在自动循迹智能车系统中更是一个关键的问题。传统的红外对管，激光管在前瞻和稳定性方面都存在一定的缺陷[5]，线性CCD是新兴的光电传感器，第八届全国飞思卡尔杯智能车比赛中就对TSL1401线性CCD传感器进行了第一次使用，与传统的红外对管，激光等光电传感器相比，线性CCD具有很多优点，尤其是在线性度和图像均匀性等方面，采集到的数据便于单片机处理。实验表明，线性CCD的前瞻比较远，能达到1.2米左右。随着对智能车辆速度增加的要求，需要跟踪处理越来越多路况，如路口，避障等。对于以上的情况，线性CCD能够利用其本身的特点有效的予以解决，尤其是在对于智能车辆的提速问题上。随着线性CCD技术的不断提高与发展，线性程度自然会持续提高，提取的图像信息会变得更加的稳定有效，前瞻会变得更宽更远，因此必定会加快推进智能车速度提高的进步[6]。本智能车系统就是基于线性CCD为基础而设计的。

2. 设计方案思路

2.1 单片机的选择

方案一：采用传统的8位40引脚单片机STC89C52作为主控芯片，该MCU是台湾宏晶公司的产品，具有低功耗，高性能，编程简单等特点，应用在很多领域。

方案二：使用飞思卡尔半导体公司的32位144引脚单片机MK60DN512VLQ10作为主控芯片，该MCU外设资源多，运算速度快，精度高。

考虑到本次智能车系统设计有一定的复杂性，要求主控芯片带有AD，PWM，正交解码等功能，并且要用到的引脚数目比较多，因此本次设计选用了K60作为主控芯片。

2.2 电机驱动的选择

方案一：使用2片BTN7971半桥芯片搭成一个全桥驱动电机，电路搭建简单，理论驱动电流达到50A,但是芯片容易发烫。

方案二：用4个MOS管LR7843构成H桥和一个MOS驱动芯片IRF2184驱动电机运转，虽然电路组建有点复杂，但是理论驱动电流能达到160A并且电路稳定不会发烫。

方案三：采用LN298电机驱动模块，电路简单，但是驱动电流只能达到2A，达不到理想驱动效果。

考虑到系统电路的稳定性，本次设计选用了方案二。

2.3 显示模块的选择

方案一：使用常用的LCD1602作为液晶显示器，显示字母和数字比较方便，控制简单，成本低，整屏能显示16*2个字符，与单片机并口连接，接线引脚多。

方案二：使用LCD12864作为液晶显示器，自带中文字库，分辨率为128*64，与单片机并口连接，但是价格昂贵。

方案三：采用0.96寸OLED12864显示器，采用4线同步串行SPI接线方式，接线简单，高分辨率，为128*64，显示效果远远超过LCD[7]。

考虑到成本，电路接线，显示效果等因素，本次设计选用了方案三。

3.总体设计

3.1系统设计要求

本课题要求设计制作一套基于线性CCD的自主行驶小车系统。本系统场地是在一个白色地面上用黑色绝缘胶带布置两根边线，两根边线内是小车要行驶的道路，小车能通过线性CCD识别出该道路，并能自主沿着道路前进，到达终点时会自动停止，终点为跟边线垂直的黑线段（黑色绝缘胶带）。在道路中间含十字路口，要求能识别出该十字路口，并正确通过该十字路口。

3.2系统总体设计方案

本系统的最小控制核心MCU为MK60DN512VLQ10；线性CCD用支架固定在小车中心离地30cm处，前瞻60cm（能准确提取边线且不失真），使用4个MOS管搭成H桥作为电机驱动模块[8]，大大增加电机驱动能力；光电传感器模块由四个ST188光电对管构成，在车身底部“一”字排开，用以检测终点。电源模块采用飞思卡尔的7.2V电池给电机供电，同时经TPS7350芯片输出5V稳压给CCD模块、编码器供电；经TL1963芯片输出6V稳压给舵机供电；经TPS7333芯片输出3.3V稳压给单片机、OLED模块供电。TSL1401线性CCD采回的图像数据传递给单片机处理，单片机利用PID算法控制舵机转变方向，调节电机速度，从而完成小车智能循迹[9]。

系统由K60为最小控制核心模块、TSL1401线性CCD模块、电源管理模块、电机驱动模块、OLED12864液晶人机交互模块、ST188光电对管模块以及电机、舵机和编码器组成。系统框图如图3-1所示。

图3-1系统框图

系统各个模块功能设计如下：

1、通过TSL1401线性CCD获取道路的一维图像信息(128个像素点的电压)，然后给单片机进行数据处理；

2、采用MK60DN512VLQ10单片机为主控芯片对外围电路进行实时控制；

3、光电对管用以检测起跑线，到达终点后，控制停车；

4、伺服舵机带动小车两前轮控制小车转向；

5、电机驱动电路驱动直流电机带动小车两后轮运转；

6、采用两片OLED12864显示屏，一片显示CCD采回的图像波形，另一片结合五向按键做人机交互界面，调节PID参数，节省多次下载程序的时间，大大提高调车效率；

7、采用500线欧姆龙编码器反馈电机转速，精确控制小车速度，有效提高系统的稳定性。

4.硬件设计

4.1飞思卡尔32位单片机MK60DN512VLQ10介绍及其最小系统

本设计使用的MCU是飞思卡尔半导体公司生产的32位MK60DN512VLQ10单片机，是飞思卡尔公司推荐的K60系列的32位增强型单片机。该芯片以飞思卡尔最新的闪存技术，并且具有功耗低，性能高，精度高等特点，特别是多路快速的16位模数转换（ADC）、数模转换（DAC）和可编程增益运放（PGA）等功能强大、高效率的信号转换，调节和控制模块。K60还具有以太网，全功能USB、硬件加密和篡改检测功能，具有丰富的模拟通信，定时和控制外设，还包括单精度浮点型运算单元，NAND闪存控制器和DRAM控制器[10]。

本设计的MK60DN512VLQ10单片机的最小系统如图4-1所示。

图4-1 K60单片机最小系统

4.2电源模块的设计

本智能小车采用飞思卡尔公司的7.2V电池给RS540直流电机供电，除此之外，系统还需要6V、5V、3.3V、12V给系统其他模块供电。

4.2.16V稳压电源设计

由于舵机需要用到6V电源给其供电，本次设计选用了德州仪器生产的TL1963A 电源管理芯片，可调输出电压范围为1.21V～20V,输出电流能达1.5A,驱动舵机的效果比较明显，其电路原理图如图4-2所示。

图4-2 6V 稳压电源原理图

通过查询芯片的Datasheet 可知，TL1963A 的输出电压计算公式为??

? ??+=12121.1R R V V out ，因此电阻R1配1K ，R2配3.9K ，经计算V OUT =1.21*(1+3.9/1)=5.929V ≈6V 。

4.2.2 5V 稳压电源设计

为了提高电池的使用时间，通常需要调节压差（输入输出电压差小），使用功耗小（静态电流小），还有就是能够有欠压信号使单片机复位，德州仪器生产的TPS73XX 系列就符合这一功能的低压差稳压器。因此5V 电源模块部分就采用TPS7350Q 芯片，最大输出电流达500mA ，输出电压精度高，噪声小，内部带过流过压保护，另一个优点就是该芯片外围元器件少，电路搭建简单，符合智能车设计要求[11]。其电路原理图如图4-3所示。

图4-3 5V 稳压电路原理图

在本系统中，5V 稳压模块给TSL1401线性CCD 模块，500线欧姆龙编码器，5V 至12V 升压模块供电，经计算CCD 模块供电需要100mA ，编码器供电只需几十mA ，升压模块需耗电100mA ，因此使用TPS7350Q 符合系统供电要求。

4.2.3 3.3V 稳压电源设计

因为MCU 的供电需要3.3V 供电，为此选用了TPS7333Q 作为电源管理IC ，输出3.3V 电压，同TPS7350Q 一样，输出精度高，带过压过热保护功能，输出电流达500mA,电路原理图如图4-4所示。

图4-4 3.3V 稳压电路原理图

输出的3.3V 电压给单片机供电以为，还给OLED 显示模块供电，单片机供电需要200mA ，由于用到了2片OLED 液晶屏，所以耗电200mA ，因此总的供电量也是合理符合的。

4.2.4 12V 稳压电源设计

由于IRF2184需要12V 供电，因此比较使用了DCDC-5V12V 升压模块，因为该模块已经集成化，所以比较稳定，纹波比较小。其外观图如图4-5所示。

图4-5 B0512S外观图

4.3电机驱动模块

电机驱动电路一般采用H全桥驱动直流电机，一般的桥式电机驱动电路分为单极性和双极性，但是双极性电路输出的电压含有比较多的交流成分，极易造成电机消磁和发烫，因此最后选择了直流电动机可逆单极型驱动器，采用4个功率元件N沟道mos管搭建而成，额定输出电流达到150A以上，驱动效果理想，使直流电机的转速和扭矩都明显提高。其电路原理图如图4-6所示。

图4-6 电机驱动原理图

为了驱动MOS管LR7843,为此使用了mos管驱动芯片IRF2184,IRF2184的驱动电压是12V，因此用了5V-12VDCDC升压模块，同时为了不让驱动部分的大电流回流到单片机最小系统把单片机烧掉，因此在单片机输出引脚和驱动之间加了隔离芯片74HC02,该芯片是4通道2输入或非门，不仅能进行电平转换，而且还能根据单片机输出高低电平控制电机正反转，使用起来更方便。

4.4OLED显示模块

本次设计用到的是OLED12864液晶显示模块，OLED的英文名为Organic Light-Emitting Dide，中文名为有机型发光二极管，该屏幕轻薄，省电，已经在MP3等播放器上得到了广泛的应用，OLED12864分辨率为128*64，能显示中文字符，大小只有0.96寸，并且成本低，实物图如图4-7所示。

图4-7 0.96寸OLED12864实物图

为了调试方便，使用了2片OLED屏幕，一片读取CCD采集的赛道信息，另一片用五向按键调节PID参数。单片机采用4线SPI驱动液晶显示，通过控制片选端CS选择驱动哪片OLED，模块的电路原理图如图4-8所示。

图4-8 OLED12864液晶屏原理图

4.5五向按键模块

本次设计的按键部分采用了五向按键，小巧方便，在硬件设计上原本需要占据5个I/O口，但本设计采用了AD按键，把5个引脚同时挂在同一个I/O口上，利用5个方向引脚电压的差异识别出哪个方向键按下，大大节省了I/O口资源。按键模块配合OLED模块使用，进行人机交互，缩短了系统调试时间，按键模块电路原理图如图4-9所示。

图4-9 五向AD按键原理图

4.6TSL1401线性CCD模块

线性CCD是本系统的核心部分，就好比小车的“眼睛”，非常重要。本次设计中的CCD模块采用了北京龙邱科技公司的TSL1401模块，如图4-10所示。

图4-10 TSL1401模块实物图

4.6.1线性CCD模块引脚定义

SI：串行输入口。经过单片机的IO口控制数据起始位。

CLK：时钟引脚。经过单片机的IO口控制电荷的转换和像素的输出。

AO：模拟输出。经过单片机的ADC口进行数据采集。

VDD：+5V电源供电。

GND：接地。

4.6.2线性CCD模块工作原理

TSL1401 线性CCD模块中有光电二极管128个成线性分布。都有积分电路与各个光电二极管相互对应，这些积分电路称为像素。采样的像素采集的灰度值与感应到的光的强度以及积分的时间成正比。

除了128个像素以外，模块还包含逻辑开关控制电路以及移位寄存器电路。每个像素的积分和复位由SI引脚控制；CLK引脚在电路中的作用是顺序输出每一个像素电压值。TSL1401线性CCD模块的内部结构如图4-11所示。

图4-11 TSL1401模块内部结构图

能够从图中看到像素电路、开关逻辑控制电路以及位移寄存器电路。还有一个增益调整模块，对输出电压可调增益，所以不需要其他放大模块进行放大，AO口输出的电压可以直接给单片机AD采样进行处理。一般都是从CIK引脚的下降沿进行AD 采样。

4.7ST188红外对管模块

终点为跟边线垂直的黑线段（黑色绝缘胶带），小车到终点是要自动检测到，如果也用CCD检测，可能会造成误判，小车转弯时也可能当作终点而停车，所以专门用了ST188红外对管模块来检测终点线，其外形如图4-11所示。

图4-11 ST188外观引脚图

其中A、K引脚是红外发射极的正负极，E、C引脚是接受极的正负极，能检测范围是4-13mm，无须接触，电路原理图如图4-12所示。

图4-12 ST188红外对管接线原理图

工作原理：当检测到黑线时（无红外线反射），A、K未导通，C、E截至，输出高电平，相反当检测到白线时（有红外线反射），A、K导通，C、E也导通，输出端为低电平[12]。

考虑到在测量过程中会出现比较大的死区电压，为此加了运放进行比较器调节，同时用电位器还能进行阈值调节，方便灵活。为了直观反映检测效果，运放输出端加了指示灯，黑线指示灯亮，白线指示灯灭。另外终点线有2段黑线，所以用了4个ST188检测，根据黑线段的间距进行合理分布，在车模底部一字排开，无论什么情况都有2个ST188同时检测到2条黑线段，使单片机控制小车停车。

4.7500线欧姆龙编码器

智能车要完成闭环控制，必须能够反馈速度，本系统采用了欧姆龙公司生产的500线E6A2-CW3C双相编码器进行反馈，转一圈能输出500个脉冲，实物图如图4-13所示。

图4-13 500线编码器实物图

编码器一段通过齿轮跟电机连在一起，模块有4根接线，褐色连5V，蓝色连GND，黑色连A相，白色连B相，利用K60的正交解码功能检测脉冲数据[13]。因为A，B 两相的相位相差90度，所以可以通过对比是A相超前还是B相超前进行正反转判断，并且也能在A、B相的上升（下降）沿进行脉冲计数，反馈给单片机测速。如果A，B相引脚直接接单片机正交解码引脚，测到的是尖峰波（并非方波），单片机不能正常处理，因此加了10k的上拉电阻后，就能得到正常的方波给单片机测速，与单片机连接口如图4-14所示。

图4-14 编码器引脚接线图

4.9SD-5舵机

小车的转向舵机使用的是北京科宇通博科技有限公司生产的SD-5舵机，它的工作电压是6V，并且自带堵转保护电路，力矩是5000g，回中较准确，定位较精确，只需300Hz频率PWM波就能进行方向控制，操作简单。SD-5舵机实物图如图4-15所示。

图4-15 SD-5舵机实物图

4.10RS-540直流电机

本系统使用了RS540大功率直流电机，工作时电压为7.2V，空载时电流为1.72A，最大的工作电流可以达到9.71A，最大工作功率达到61.75W，完全符合小车速度需要，电机实物图如图4-16所示；

图4-16 RS540电机实物图

4.11小车车模

使用飞思卡尔智能汽车竞赛的B模型，实物图如图4-17所示。

图4-17 飞思卡尔比赛B车模

5.软件设计

硬件平台搭好后，就需要软件的开发，小车只有通过软硬件的有机结果，才能实现自动循迹功能，本系统软件开发使用的是IAR，其界面如图所示，IAR有多种版本，有编译ARV的，stm8的，ARM的，因为K60是ARM内核的，因此选择了IAR For ARM版本进行安装。IAR是目前最简单和最完备的嵌入式开发工具，利用IAR开发有许多优点，能够最大化地优化代码，节约硬件资源，大大地降低开发成本。本次设计软件编写使用了通俗易懂，可移植性较强的C语言，在电脑上安装好IAR软件后，根据使用说明建立一个工程项目，进行程序编写和在线调试，界面如图5-1所示，详细的源程序见附件。

图5-1 IAR界面图

5.1总体方案

程序主要要实现的功能是：通过TSL1401线性CCD传感器采集的128个一维图像信息，传递给单片机进行判断处理，单片机根据PD算法，判断小车与中线的偏差输出PWM波控制舵机打角多少，使小车始终沿跑道中线行驶，为了控制小车的速度，使小车能闭环稳定跑，通过反馈编码器的转速给单片机，单片机利用PI算法同样输出PWM波控制电机速率。当ST188光电对管模块检测到终点线后，单片机控制小车停车。为了在调试过程中方便快捷，提高效率，利用了K60的flash擦写功能，在OLED液晶屏上使用五向按键改变PID参数，直接在小车上调试，省去了多次下载程序的时间，并且能对调试理想的参数进行保存，下次开机后还能读取上一次保存的参数，明显提高调车效率。

5.2主程序设计

程序上电后，先关总中断，然后串口、CCD、PIT0、FTM0、FTM1、OLED、ADC、FLASH、按键等各模块初始化，然后开总中断，进入main函数，开始各子函数调用。首先开始CCD的采集，CCD采集的128个像素点的电压通过单片机AD采样，然后

二值化后通过PD 算法输出PWM 波控制舵机打角转向，编码器正交解码返回脉冲数给单片机通过PI 算法输出PWM 波给电机调速，并且执行AD 按键扫描程序，执行按键事件进行OLED 屏幕参数调节（PID 参数）。以上子程序都在PIT0中断中执行，在PIT0中断中，1ms 进一次中断，为一个控制周期，共分4个控制周期，第一个控制周期内CCD 曝光计算，第二个控制周期内进行获取编码器脉冲数后进行PI 调速，第三个周期内进行内进行位置解算,PD 算法控制舵机转向，第四个周期为按键扫描，进行人机互动。Main 函数内做两件事：一是判断终点线，一旦检测到，就直接电机不使能停车；二是CCD 计算曝光时间（20ms 一次），会在后面CCD 部分讲到 。主程序框图如图5-2所示。

图5-2 系统主程序框图

5.3 各子程序模块设计

接下来开始介绍各个子程序部分的设计，由于各程序中基本上都用到了PID 控制算法，先介绍下PID 控制原理。

PID 控制算法为使用较早，较为经典的控制方法之一。PID 控制器根据系统的过去信息（I ）、当前信息（P ）和将来信息（D ）三个参数进行控制。P 为系统偏差的比例，I 为系统偏差的积分，D 为系统偏差的微分。PID 控制算法优点有很多，使用起来方便简单，鲁棒性能比较好，因此在工业上得到了普遍的运用。

PID 控制系统由三部分组成，分别是控制对象、执行机构以及模拟调节器，如图5-3所示。

图5-3 PID 控制系统原理框图

PID 调节器是一种线性调节器，e （t ）为给定的量r （t ）和实际输出的量c （t ）之间的偏差：()()()t c t r t e -= ，然后将系统偏差的比例(P)、积分(I)、微分(D)根据某种线性组合关系组合成控制量，最终对控制量一起控制，因此叫PID 调节器[14]。但

是在实际生产应用中，常常会根据相关控制要求需要将P 、I 、D 进行适当的组合，来达到有效控制被控对象的目的。比如，P 调节器，PI 调节器，PD 调节器，PID 调节器等。

一般的PID 调节控制规律为：])()(1)([)(0dt

t de T dt t e T t e K t u D t I p ++=? 上式中，K P 是比例系数，K I 是积分常数，K D 是微分常数。

各校正环节的在系统控制中作用是：

（1）比例环节（P ）：作用是一旦系统出现了偏差，比例调节环节马上进行调节，抑制偏差的产生，但是也有缺点，就是系统会产生余差；

（2）积分环节（I ）：主要作用是为了消除比例环节产生的余差，提高系统的无差度。积分环节作用与积分时间常数有关系，积分时间常数越大，那么积分作用就越弱，反之则正好相反；

（3）微分环节（D ）：主要反映了系统的偏差变化的预见性，在系统偏差还未变大时就产生抑制调节，有效减少系统的调节时间。

5.3.1 TSL1401线性CCD 程序设计

由于因线性CCD 的输出信号与周围环境的光线有关系，一般情况下，晚上灯光效果比白天会使输出的电压低，当线性CCD 背对光线比正对着光线输出电压低，输出电压大小和CCD 曝光量大小直接相关。曝光量取决于CCD 模块所选用的镜头光圈的大小和程序所控制CDD 曝光时间多少，因为镜头光圈大小已经固定不能改变，因此使用软件改变曝光时间，本次设计采用了自适应曝光时间算法调节曝光量[15]。程序流程图如图5-4所示。

图5-4 CCD 自适应曝光曝光程序框图

曝光控制原理：先发送一次启动和移位时序信号，因为TSL1401每128个时钟序列的前18个周期是内部复位，先将积分电压清零，第18个周期以后才开始积分。中断程序就是根据曝光时间的大小在适合的曝光点使CCD 复位。下一个20ms 到达后再重采样数据，采集到数据对应的曝光时间就是系统输出内部复位序列到下一次采样的时间。例如曝光时间为7ms ，则曝光点为13ms 。自适应曝光程序有效解决了CCD 的曝光受光线影响问题。

5.3.2 电机PI 算法调速程序设计

PI 调节器是在自动控制中常常用到，该调节器主要作用是能快速进行系统反映，并且能消除系统稳态误差，结合了比例环节和积分环节两者的优点。本次设计用到了

增量式PI 调节器，其公式为()()()()??

????+--=?k e T T k e k e K k u I P 1，其中K P 为比例系数，K I 为积分时间，输出为位置增量。

软件编写思路：进入中断后，通过正交解码获取的编码器脉冲数和给定的速度的偏差，利用PI 算法输出PWM 波控制电机，不断的刷新，能在短时间内控制电机速度达到预期设定的速度。

流程图如图5-5所示。

图5-5 电机PI 调速算法程序框图

5.3.3 舵机PD 算法转向程序设计

小车转向通过单片机输出PWM 波占空比控制舵机打角，在数据处理过程中，使用边缘检测法检测轨迹。将CCD 传感器取回的128个数据点根据阈值二值化后依次坐标化为0至127，由于白线为高电平（1），黑线为低电平（0），就可以得到左右边界值。依据“中线位置＝（左边位置＋右边位置）/2”，就得到中线位置。根据中线位置与中间值63的偏差，对舵机进行控制，对于一侧丢现情况（弯道）可以使用补线方法求中线位置，对于两侧都丢现（十字路口）可以采用保持上次中线位置值作为本次中线位置值使用的方法。

舵机转向利用PD 算法，其核心算法为()()()[]()k e K k e k e K k u D P *1+--=?，其中K P 为比例系数，K D 为微分系数。使用PD 算法可用既能快速反映抑制偏差，又有偏差预见性，防止偏差的变大，程序流程图如图5-6所示。

图5-6 舵机PD 转向控制算法

5.3.4 五向按键扫描程序部分设计

使用了AD 扫描按键，在PIT 中断中设置4ms 的按键中断，根据各按键的电压不同，判断当前被按下的是什么键，利用switch 、case 函数对不同的按键进行相应事件