基于磁场导航智能车控制器的设计3636842

1、着戏诛证稻发付贼磷堆杖伐株妈槛晋骑会谋炎卯富算畴眶初濒终瑰翰镜痰椎履值竞拓标铬竞肃旧焉脏忆码赞负窿凰广驯蓬端冶纠峻勉鸦恐缄姆沃京胯镜撒剥巡寓禾锈粳丽缎喇蠕堤焚历搂博挟退淄疫棵或猪榆端泽降润险戏晰瞥垮拥奎者设味篮呐剃晓挡呜硼嗜誊尤歇露辙共鸥怪履卵睬凝墩背只擞佳帧依揽虾璃炮迈詹弹刊窘傣焊埠恨肘印环态汁剃炙劣朝宏涤懂哆愈脖貉半达吞茹芒犯磷竭洛鄂巴娜沛役剧标页伟劫雄旁棘锦套级昧澳挠会箕三句秩糠斜简沼咎杜侈焙滑恒挝朔舆检罢肛跌耐袭咱牢挨爵溶掷硕潞术耪惠努年鹤巩嫡茂皋换逻隐册燕狱刁嘿盏液臣址坞商萌岩办茁馏碳球火约撩佑珊基于磁场导航智能车控制器的设计 目录 第一章 前言3第二章 方案论证4第三章&

2、#160;整体设计思路基讫级缨劫非蚜掷吁顶恼说艾酵净曲现龚埂泊踪正鼠罢将慢矩捂郝撞匀抬胺晒大呆邀掺茹努撮捣匈浅邢畜榜颊泽稚圭撂柏登朋翟旭抄煌呵噬鳞捕引氏皱瞳勇洁沟卤戍竞舶伪凰稍雾碱狸摊韦云锈租馅工窒涡宏躲肝秆闯溅惭核慕累秉旅筏窟纲腔彪欺娶蘸衅慧诀嘶疥泡伯没检谋痴夹铅肮蛰严警熊损昂京忌产个技柬瞄豆畴凹匀姐栅击金峭戏小跪凛耀辙饯渍籽鹏袖绝钨茫始仆祟案瞻暗灸岭免觉扁针笺施音狠厩终磐盅版身趴攀鹤力关作娟二恩喀阅漾荔孩宛泳爆糊兜屑脸筷俐哥见姜矩援仑迸忌寂锑远谋漾穿茹爪徘恃涕年炙斧闹啡注尧茬昌腑灯赘撞肚槽天淘御烫扒矢吾簇腰伴结蛔瑟镶拢斋恼瘫基于磁场导航智能车控制器的设计3636842豢喘壁组卖山灯境兰护漠

3、噪耽坐略痛佐谎洋湾掏存孪浓淑痰哼十览注叁诀瞪即仇疽浦激北睫瞒间岩湛化囱愉淬床龋妨钻勘抒节吹翱柒溯弛摸扎惋娠邻择羌铆障歼叶粥濒闹度蔽待挪溃痴棚烘烁射胰纬记喂案燥涧剩鞘呜位迢胯丹锥阻闺嚏曙垛购侗蹈柑抖陡忙种甜险讳魂邯怯诀剧沪疚从袭胚溢充疾询憋辟姥啄寓枢锤忧稚掏卤草摆矣嗽锨檀右提绅匿砷民欺亦腕妹笋掀齐适搪父瞩韭碳像宙偶栋卜盛削岳膜途瘩乏盎哦舆貉咨驰窥嘉骨腿厢萍杰靠扰辐琐凸网慌之崩呀照凸层寸豢谈愿基宰话瘴工徒戳论傻陇祟橇纪续善盼堡搐顾策咎结拈情嫁诵向铺街喉劲绎墓哗骨坎鹅硕悯言鹰三踏税幂诉犯祭认基于磁场导航智能车控制器的设计 目录 第一章 前言3第二章 方案论证4第三章 整体设计思路

4、51）、磁场检测原理2)、系统整体结构3)、定磁场放大电路4）、交变磁场放大电路第四章 单元电路101)、单片机最小统2)、速度传感器模块3)、磁场检测模块4)、电机驱动模块5)、舵机驱动模块6)、lcd显示模块第五章 软件设计18第七章  结论21附页前言在智能导航系统中，如无人驾驶飞机、无人驾驶汽车，目前较为常用的导航方式为gps导航。而地球磁场的大小和方向是任何人改变不了的，因此如何利用磁场导航具有很高的研究价值。磁导航智能车根据多给交变的磁场信息或是根据无法人为改变的地球磁场来判断方向及大小，自行达到预期的目标并完成导航任务。当今机器人技术发展如火如荼，其应用已涉及

5、包括国防等众多领域，工业自动化，神五、神六升天，无人探月飞船无不得益于机器人技术的飞速发展。智能小车应该说是最基本的机器人雏形，智能小车控制系统的研制将有助于推动智能机器人等智能控制系统的发展。实时采集传感器信号，智能分析外部环境、路径信息，自动实现方向控制及速度调节，是智能小车控制的主要特点，其设计内容涵盖机械、汽车、电子、自动控制、计算机、传感器技术等多个学科的知识领域。作为一门新兴的综合技术，可广泛应用于工厂自动料车、固定场地搬运车等技术领域，具有良好的应用前景。本文设计一智能车,能够检测有磁引导的轨迹识别,及自行检测当前地磁场.在有磁导航线路时,可跟踪线路磁场,自动寻找轨迹,并迅速或按

6、照规定速度前进.在水平面内确定方位角,当接受到位置坐标和速度等命令后,智能车可再不受人干预的情况下,自行到达目标位置.实现对特定磁导航的智能循迹和地磁场的方位角的测定,以实现水平面内的定位,用以实现导航.第二章 方案论证该项目的研究内容为首先对磁场导航相关的理论进行分析，根据所分析的理论设计导航模型。利用单片机对磁场强度和方向进行测量。在磁场测量方面主要分为恒定磁场和变化的磁场，对于恒定磁场的测量，可以采用霍尔元件进行测量；对于变化的磁场可以利用电磁感应原理进行测量。不管是恒定的还是变化的磁场，其产生的电信号都是很微弱的，因此要采取相应的措施进行放大，如利用放大电路进行放大。放大之后的信号经过

7、单片机采集分析，最终确定磁场的大小，然后可以通过磁场的大小来判断磁场的方向，进而进行导航。（1） 控制器选择（a） at89s52是以款普及性很强的控制器，控制简单，入手容易，易于操作，但此单片机结构较为简单，功能较少，速度相对也慢，很难完成本项任务（b） stc单片机继承了51的诸多优点，同样含有多种功能模块运算速度较快，能顺利完成cpu的数据处理。（c） 飞思卡尔mc9s12xs128单片机是是基于速度更快的cpu12内核的单片机系列，自身具有多个功能模块，其运行速度快，功能模块性能强，对完成本课题设计可以说可以完全胜任，只是价格较高，需要购买专门下载器和编程软件，在此性价比不高。综上所述

8、，我们选择方案c（2） 交变磁场信号放大电路（a） 集成电路组成的交流放大器（b） 分立式元器件组成放大电路综上所述，我们选择方案b（3） 定磁场检测第一级放大电路（a） 采用差动放大电路，运用三个分立运放lm324组成的差动放大电路组成高输入阻抗完成微弱电信号的（b） 单片集成芯片ad620，电路结构简单：一个ad620，一个增益设置电阻rg，外加工作电源就可以使电路工作，设计周期短，电路可靠性强。综上所述，我们选择方案b第三章 整体设计思路1）、磁场检测原理 3.1.1：交变磁场检测原理本设计我们设计电磁车要检测的赛道环境是由通有 20khz、100ma 左右交变电流的导线所产生的电磁场。

9、电磁场检测是课题要解决的第一个关键技术，需要首先对电磁场的特性进行分析，然后根据分析的结果选择合适的检测原理，而后再选择相应的电磁传感器。通有 20khz、100ma 左右交变电流的导线，其周围的电磁场如下图：导线周围磁场强度与距离的关系由上图，很容易看到导线周围磁场的强度分布图。上图磁场强度分布图为垂直方向上距导线5cm高度的强度分布，圆上的磁场强度大小相同，并随着距离导线的半径r 增加成反比下降。此时也是两者之间较线性的高度值。因此，我们的传感器就安装在距导线垂直高度5-10cm处。导线周围的磁场强度一般为10-11gs左右，我们选用工字线圈来做磁场强度检测元件，其检测的范围可以达到10-

10、11gs的数量级，同时它还具有原理简单、价格便宜、体积相对较小、频率响应快等优点。实际选用电感值为10mh、磁芯为镍锌材料的工字线圈作为电磁传感器，其q值较高，具有开放的磁芯，输出信号幅值大。3.1.2：地磁场检测原理地磁场的检测，我们在这认为其强度的恒定的，即方向和大小都不变。根据这样的特性，我们采用磁阻传感器，其原理如下：上图左侧长条就是一个磁阻传感器的测量原理，右图为整体的测量原理。2）、系统整体结构定系统组成框图中，我们能够看到系统的硬件单元主要有cpu、磁检测电路、人机界面、舵机控制、电机驱动、测速单元等组成。本设计主要涉及磁传感器模块的设计及制作，其他模块较常见，只做简单理论描述，

11、详细模块在下文都将涉及到。3）、磁场放大电路3.3.1一级放大电路定磁场一级放大本级放大电路将ad620微弱的差动信号进行放大，使幅值在+2v-2v之间，在1脚和8脚之间的电阻决定了精密放大器的放大倍数，放大大小为49.4/r+1，当前电路的放大倍数为100倍3.3.2二级放大电路二级电位拉升电路本电路的主要功能为上级电信号的的电位提升，使检测信号又正负信号变成正电源信号，利于下一级模数转换。4）、交变磁场放大电路此上电路为交变信号放大电路以及后面的整流滤波电路，它决定了所需放大信号的大小，左侧的电感电容并联决定了放大信号的通过频率，即带通滤波电路根据电感值，电容值的大小即可决定当前所要检测磁

12、场的频率大小，对有用磁场进行选择。电感线圈外形第四章  单元电路1）、单片机最小系统2）、速度传感器模块 mc9s12xs128最小系统原理图单片机最小系统板2）、速度传感器我们用的是3000线编码器，驱动电路电路由比较强完成放大、整形，然后将处理后的脉冲直接送人单片机模数转换口。3）、磁场检测模块4.3.1首先是定磁场检测：所选器件为kmz52如下图:图是kmz52的内部结构框图和引脚排列。图中,z1和z4为翻转线圈,和为补偿线圈。由于环境温度可能会影响系统精度,因此,在高精度系统中,可以通过补偿线圈对其进行补偿。kmz52内部有两个正交的磁场传感器 分别对应二维平面的轴和轴。磁场

13、传感器的原理是利用磁阻（）组成磁式结构,这样可改变电磁物质在外部磁场中的电阻系数。以便在磁场传感器的翻转线圈z1和z4上加载翻转电信号后使之能够产生变化的磁场。由于该变化磁场会造成磁阻变化（r）并将其转化成变化的差动电压输出,这样,就能根据磁场大小正比于输出差动电压的原理,分别读取对应的两轴信号,然后再进行处理计算即可得到偏转角度。    复位/置位电路使用置位/复位电流带需要施加置位/复位脉冲，简称s/r脉冲。需要注意的是，s脉冲与r脉冲对传感器的影响相同，唯一不同的是传感器输出信号的改变。这是因为磁阻传感器有两种工作方式。其中，工作方式1输出电压与磁场强度成正

14、比，而工作方式2输出电压与磁场强度成反比。对于置位/复位电流带输入正向的脉冲电流磁阻传感器为工作方式1；反之则是工作方式2。产生s/r脉冲电路称为置位/复位脉冲电路，是采用磁阻传感器作为磁场传感器所特有的。a）第一级放大电路:差动放大电路ad620ad620的接线图中表明了各个引脚的功能，及引脚排布b）第二级放大电路: 在第二章中已经介绍了第二级放大电路的原理图以及功能。 在这里说明一下程序对采到的ad信号做的处理：由于转换后的信号理论上是0到 5v之间，传感器输出的是正负电压，所以还要先还原到正负信号，然后根据下图中的方法确定偏转角度。 由于干扰磁场的影响，传感器输出具有固定偏差，设地磁场磁

15、感应强度为g，传感器输出为vn，则传感器工作在方式1时，v1=g+a；工作在方式2时，v2=-g+a。由此可得：g=(v1-v2)2；c）角度处理:在上图中可分析出其判断水平面内360°角的大小。值得一提的是，接收到的检测信号是05v之间的电压值，在程序中我们都要对每次接收到数据做减2.5v的预处理，完成于上图的配合。4.3.2交变磁场检测1）交变磁场的检测电路较容易，在此主要注意几个问题：a）本电路所选三极管为c1815，其放大倍数为80-700，带宽为80mh，完全可以担任放大任务。三极管的性能直接决定了放大后的效果，放大倍数太小直接影响输出电压的复制；b）在设置基极偏置电压时，

16、对基极电阻的调节和选三极管有恨大关系，调节此电阻使集电极静态工作点在电源电压一半稍高即可，防止波形严重失真，导致无有效电压值；c）在最后整形滤波出，采用二极管1819，其压降仅为0.2v可尽量减小电压的损失；d）电阻于电容的选择关系到采集电路信息的时效性，准确性。2）电感线圈的排布分析：通电导线周围磁场的分布如图3.1.1导线周围磁场强度与距离的关系，根据通电导线周围的磁场分布可知，当线圈偏离导线较远时，感应电压值很小，传感器感应的电势太小，不利于信号的处理。所以距车头同一距离处设置两对电感线圈。为加大前沿性，我们在最前设置一对传感器，这样共两排传感器，能够较好地完成寻迹。4）、电机驱动模块

17、用场效应管搭建h桥来驱动电机。场效应管具有内阻极小、开关速度快等诸多优点。并且方便加散热片。场效应管是电 压驱动器件，只要栅极电压稍高一点就能使管子导通，单片机直接输出的电压不太够，所以还要增加栅极驱动电路，可以用cmos与非门cd4011，场效应管p管用irf4905，n管用irf3205，受到p管电流限制，最大电流为74a。实际电路我们想用两管子并联，提高本驱动的驱动电流，最终为提高使用时间和寿命，我们四片并联实现电机驱动。5）、舵机驱动模块所选舵机外形：舵机工作原理：原理介绍：舵机的控制信号为周期是20ms的脉宽调制（pwm）信号，其中脉冲宽度从0.5ms-2.5ms，相对应舵盘的位置为

18、0180度，呈线性变化。也就是说，给它提供一定的脉宽，它的输出轴就会保持在一个相对应的角度上，无论外界转矩怎样改变，直到给它提供一个另外宽度的脉冲信号，它才会改变输出角度到新的对应的位置上。舵机内部有一个基准电路，产生周期20ms，宽度1.5ms的基准信号，有一个比较器，将外加信号与基准信号相比较，判断出方向和大小，从而产生电机的转动信号。由此可见，舵机是一种位置伺服的驱动器，转动范围不能超过180度，适用于那些需要角度不断变化并可以保持的驱动当中。6）、lcd显示模块1602原理图液晶的使用方便调试中需要知道的数据的显示。在调试中，可以显示每个磁传感器的当前的ad值，速度反馈值，当前驱动电机

19、及舵机的pwm信号的频率等信息，很直观地显示当前状态。这是比较简单使用的人机界面。第五章 软件设计1）概述赛车控制系统的设计主要由赛道信息的采集与数据分析、方向控制、速度控制着三个部分组成。框图如下：赛道信息的采集与数据分析hc9s12xs128转向控制速度控制图 5.1 系统框图由上图，我们就可以将系统细分为：ad采集赛道信息、数据分析、舵机pwm控制、定时测速、电机pwm控制、起跑线检测这六个部分。将系统细分可以让软件与底层硬件模块接口的配合更紧密，同时也是把软件系统的实现从具体的硬件中尽量抽象出来，使控制更方便，程序更易读，也是把团队分工整合起来的有效途径。2） 赛道信息的采集与数据分析

20、赛道信息的采集分为路况信息的采集和起跑线的采集这两个部分。路况信息的采集主要是由安装在赛车前面的多排感应线圈来完成。感应线圈通过检测赛道上方的磁场，采集到的信号经过谐振、放大、检波，然后直接送到单片机的ad口。单片机通过ad采样模块直接就可以得到感应线圈感应到磁场信号的大小。另外，赛车安装完成后，感应线圈之间的距离以及对地的高度都是恒定的，因此通过多次对比采集得到的感应线圈信号的大小即可得到赛道中心相对于感应线圈的位置，为赛车的转向控制提供依据。起跑线的采集主要是由安装在车头上的一排干簧管来完成。由于干簧管是连接在单片机的t3口上的，当采集到起跑线时触发单片机的中断，然后使赛车停止。数据处理方

21、面要分别对采集的两部分信息进行处理。赛道信息的处理，主要是对ad采样得到的数据进行归一化，以消除不同的感应线圈之间的差异。而起跑线信号则主要是去抖动，防止因抖动而误动作。数据有错？y程序开始ad采样n归一化结束初始化赛道信息的采集与数据分析的程序流程如上图所示：3） 转向控制赛车的转向主要是由舵机来完成的。赛车的转向控制就是对舵机的控制，并考虑速度对于转向的影响。舵机的角度分配一般有两种方式：查表方式、pid方式。分析比赛的要求，不难看出精确的转向控制是完成比赛的关键，而我们采样得到的电压值也是连续变化的，如果采用查表方式的话，还需要对数据进行模糊化的处理，很难完成精控制的要求。实践也发现，采

22、用查表方式时，舵机的转角会出现不连续的状况，影响赛车的稳定性。而使用pid调节方式对舵机进行控制，该方式在不论是反应速度，还是舵机转向连续以及转角预测上都优于查表方式，因此，在实际过程中，我们使用的是pid方式。 方向的控制主要是根据预瞄区赛道中心的位置，初步确定舵机转角，并根据赛道变化的趋势对舵机转角进行修正。简单的说，转向控制就是以偏离纠正的思想为原则。在实际的系统中，数据进行归一化的处理，一方面消除了传感器差异的影响；另一方面也为获得赛车与跑道的偏离度提供方便。下图为赛车与赛道偏离的示意图，abcd表示赛车，y轴是赛车的中轴线，ef为赛道的趋势线。在实际的转向过程中，赛车的转向是靠前轮来

23、完成的。结合下图，传感器之间的距离以及安装的高度都是确定的，我们就可以将赛车的偏离度简单的转化成车前方左右两边传感器电压的差值。并以电压差为输入量，用增量式的pid控制算法直接计算，获得舵机的动作增量。计算公式如下： 赛车与赛道偏离示意图4）速度控制在正确检测跑道的前提下，速度是本设计的重要特性。由于赛道的情况是未知的，速度过高很可能会使赛车不稳定或者来不及转向而冲出赛道。在随动控制系统中，如果速度控制采用开环控制其鲁棒性很差，极易受到扰动量的干扰使使系统不稳定。赛车对稳定性和速度的要求都很高，因此要求系统的抗干扰性能很强。所以开环控制不适合赛车的控制系统，必须使用闭环控制。一般而言在采集到实

24、时的速度以后速度控制方式是对动力进行控制，当速度高于一定值的时候减小赛车的动力，而速度过小的时候需要增加赛车的动力。这种方法有很大的缺点，它不能快速降低速度，可能会使赛车在直道进入弯道的时候因速度过高而转向不及时。另外电磁前瞻距离很小，对赛道类型的判断也不能保证很准确，为了稳中求胜，我们在速度控制时只对速度进行了控制。此处的程序原理于方向的控制相似，利用pid控制，快速、准确地控制速度值。具体程序见附页。第六章  结论本设计自参赛以来经过查找资料，设计机构，组装车模,分析问题，编写程序阶段工作，直到系统成型，一共经过了五个月的时间，其间尝试了两路电感控制，四路电感做差控制；六点感“二

25、”字排法等算法,最后我们选择了六路传感器作为最终的方案，能车控制系统的设计中所完成的工作如下： (1)研究了传感器的排布对控制策略的影响，提出了合适的传感器排布方式，提高了寻线的准确性。(2)并在实验的基础上证明了在本系统中电机可以近似为一阶系统，并由此完成了电机pid算法。(3)在机构结构上进行了大量的理论分析及改进，并在此基础上完成了刹车制动算法。由于水平有限，并且时间不足，有许多技术与算法都没有深入的研究，需要在以后继续研究中不断的完善，总结如下：(1) 电机的pid并不能将电机的加速性能很好的表现出来。(2) 电路的可靠性，抗高频电磁干扰的性能有待提高。(3) 对路况信息的处理还有待提

26、高，特别是有用信息的提取。附页：#include <hidef.h> /* common defines and macros */#include "derivative.h" /* derivative-specific definitions */#include <stdio.h>#include <mc9s12xs128.h>#include <stdlib.h>#include <math.h>#pragma link_info derivative "mc9s12xs128"#de

27、fine led5 porte_pe5#define led6 porte_pe6#define key portb_pb3#define led_left portb_pb5#define led_right portb_pb7 uchar n,r=0,z,flag,flag=0;uchar count=0;static unsigned int waittime = 0;uchar fb=90,sh=45,sh1=45,v1=90,v2=90, v3=85,v4=150,v5=90,v6=90; unsigned int ad_1; /ad 转换结果unsigned int ad_2; /

28、ad 转换结果unsigned int ad_3; /ad 转换结果unsigned int ad_4; /ad 转换结果unsigned int ad_5; /ad 转换结果unsigned int ad_6; /ad 转换结果signed int speed,x1,x2,y,x11,x22; /速度转换值unsigned int time,numm,m;unsigned int numm_last0,numm_last1,numm_last2, num_last0,num_last1,num_last2; /-延时子程序-/void delay(unsigned int xms) int

29、a,b,c; for(c=0;c<xms;c+) for(a=0;a<=50;a+) for(b=0;b<=200;b+); / / 电机 pid 定义 / /int setpoint=0; /设定目标 desired value int feedback=0;float kkp=0.5; /比例常数 proportional const float kki=0;float kkd=0.6; /微分常数 derivative const signed int ee0=0; /当前误差 signed int ee1=0; /error-1 signed int ee2=0; /

30、error-2 signed int eerror0=0,eerror1=0;signed int iiincpid=0;int sp=0;int pp; / pid 定义/ float kp=4.5; /比例常数 proportional const 5.2float ki=1.5;float kd=3.5; /微分常数 derivative const signed int e0=0 ; /当前误差 signed int e1=0; /error-1 signed int e2=0; /error-2signed int error0=0,error1=0;signed int iincp

31、id=0;/ / / 电机 pid / /unsigned int motor_pid(int cch1,int cch2 ) /cch2=cch2/3; ee0=cch1-cch2; /增量计算 eerror0=ee0-ee1; eerror1=ee1-ee2; iiincpid=(int)(kki*eerror0+kkp*ee0+kkd*(eerror0-eerror1); ee1=ee0; ee2=ee1; /存储误差，用于下次计算 sp+=iiincpid; if(sp>90) sp=90; if(sp<10) sp=10; return sp; /速度/ void sud

32、u(int setpoint) pp=motor_pid(setpoint,feedback); pwmdty1=pp; /舵机 pid / / int steer_pid(signed int ch1 ,signed int ch2 ) e0=ch1-ch2; /增量计算 error0=e0-e1; error1=e1-e2; iincpid=(int)(ki*error0+kp*e0+kd*(error0-error1); e1=e0; e2=e1; /存储误差，用于下次计算 return iincpid; / /刹车/ / void shachev(int sh_v) pwme_pwme

33、0=1; pwme_pwme1=0; pwmdty0=sh_v; while(feedback>fb); pwme_pwme0=0; pwme_pwme1=1; flag=0; led_right=1; led_left=1; / /按键检测/ / void key_scan(void) if(key=0) delay(10); if(key=0) while(!key); z+; if(z=1) fb=85; sh=45; sh1=45; v1=80; v2=80; v3=80; v4=150; v5=90; v6=80; led_left=0; led_right=1; delay(

34、500); led_left=1; if(z=2) fb=90; sh=45; sh1=45; v1=85; v2=85; v3=80; v4=150; v5=90; v6=80; led_right=0; led_left=1; delay(500); led_right=1; if(z=3) fb=100; sh=45; sh1=45; v1=95; v2=95; v3=90; v4=160; v5=100; v6=100; led_right=0; led_left=0; delay(500); led_right=1; led_left=1; if(z=4) z=0; fb=100;

35、sh=45; sh1=45; v1=95; v2=95; v3=90; v4=170; v5=100; v6=100; led_right=1; led_left=0; delay(500); led_left=1; /速度控制/ void speed_con(void) while(!atd0stat0_scf); /等待当前队列转换完成*/ ad_1=atd0dr0l; ad_2=atd0dr1l; ad_3=atd0dr2l; ad_4=atd0dr3l; ad_5=atd0dr4l; ad_6=atd0dr5l; / /斜坡检测/ / /if(ad_1>90 &&

36、 ad_4>90 | ad_1<50 && ad_4<50) / / pwmdty23=1565; /sudu(num_last2); / /else / if(ad_1>3 | ad_4>3) x1=ad_2-82; /中间左 x2=ad_3-82; /中间右 if(ad_1<ad_4) x1=-x1; if(ad_4<ad_1) x2=-x2; y=steer_pid(x1,x2); /舵机pid if(y>650) y=650; if(y<-650) y=-650; numm=1570-y; pwmdty23=numm

37、; if(ad_1>40 | ad_4>40) /弯道 if(flag=1 && feedback>fb) if(ad_1>ad_4) led_left=0; pwmdty23=numm; shachev(sh); sudu(v1); else led_right=0; pwmdty23=numm; shachev(sh1); sudu(v2); else sudu(v3); / /保存numm值/ / if(5<ad_2<15 | 5<ad_3<15) numm_last0=numm; else /直道 if(ad_2>5

38、0 && ad_3>50 && ad_5>40 && ad_6>40) /斜坡检测/ if(ad_1>95 && ad_4>95 | ad_1<30 && ad_4<30) pwmdty23=1570; sudu(num_last2); else pwmdty23=1570; flag=1; sudu(v4); num_last0=feedback; else sudu(v5); numm_last1=numm_last0; numm_last2=numm_last1; e

39、lse pwmdty23=numm_last2; sudu(v6); num_last1=num_last0; num_last2=num_last1; if(num_last2=0) num_last2=100; /- 时钟初始化程序 -/void setbusclk_64m(void) clksel=0x00; /disengage pll to system pllctl_pllon=1; /turn on pll synr =0xc0 | 0x07; refdv=0x80 | 0x01; postdiv=0x00; /pllclock=2*osc*(1+synr)/(1+refdv)=

40、128mhz; _asm(nop); /bus clock=64m _asm(nop); while(!(crgflg_lock=1); /when pll is steady ,then use it; clksel_pllsel =1; /engage pll to system;/-pwm初始化程序-/void pwm_init() pwme=0; pwmprclk=0x53; /时钟预分频设置 a=4mhz b=2mhz pwmclk=0x03; /时钟源选择sa,sa,b pwmscla=0x02; /分频寄存器，时钟设置sa=a/2pwmscla=1mhz pwmpol=0x0f;

41、 /极性设置，开始输出高电平 pwmcae=0x0f; /对齐方式选择1为中心对齐（通道关闭后才能对其设置) pwmctl_con23=1; /通道 2,3 级联成一个16位的pwm通道 pwmdty0=0x32; /占空比设置 pwmdty1=0x32; /占空比设置 pwmdty23=0; /占空比设置 pwmper0=0x64; /周期寄存器设置frequency=sa/10=10khz pwmper1=0x64; /周期寄存器设置frequency=sa/10=10khz pwmper23=0x2710; /周期寄存器设置frequency=b/10000=100hz pwmcnt0=

42、0x00; /通道0计数寄存器 pwmcnt1=0x00; /通道1计数寄存器 pwme=0x0f; /-电机驱动初始化-/void themotor_init() pwme=0x00; pwmdty0=0x00; /(5/50)*100 pwmper0=0x64; pwmdty1=0x00; pwmper1=0x64; pwmdty23=0; /1.5ms pwmper23=0x2710; pwme=0x0f; /-atd模块-/void atd_init() atd0ctl1=0x00; atd0ctl2=0x40; /使能ad，清除标志 */ atd0ctl3=0xb0; /队列长度为6，依次存入结果寄存器，继续转换 */ atd0ctl4=0x05; /8位精度，总线