河海大学“凌波号”智能小车组技术报告

1、河海大学“飞思卡尔”杯全国大学生智能汽车竞赛技术报告 学 校： 河海大学 队伍名称：“凌波号 ”智能小车组参赛队员： 张思德，郁舒阳，赖柄霖，张华山，周梦茜 目录第一章 摘要3第二章 作品完成功能42.1 舵机的固定与安装42.2 前轮的调整52.3 差速的调整62.4 整车重心的调整7第三章 系统方案论证93.1 电机驱动模块方案选取103.2 H桥式电机驱动电路原理 103.3 传感器布局103.3.1确定导线位置布局103.3.2前瞻设计10第四章 硬件结构设计及实现124.1 控制器模块134.2 路径识别模块134.2.1感应线圈134.2.2信号选频放大134.2.3检波

2、整流154.3 电源模块164.4 舵机使能控制电路164.5 电机驱动模块17第五章 软件结构设计及实现185.1 控制总流程195.2 导线位置提取195.3 系统控制算法205.3.1舵机转角控制算法205.3.2电机转速控制算法21第六章 作品测试数据22第七章 不足及今后改进方向22附录A 程序代码23第一章 摘要   第九届“飞思卡尔杯”全国大学生智能汽车邀请赛规定参赛选手须使用竞赛秘书处统一指定并负责采购竞赛车模套件，采用飞思卡尔半导体公司的8位、16位微控制器作为核心控制单元，自主构思控制方案进行系统设计，包括传感器信号采集处理、动力电机驱动、转向舵机控制以

3、及控制算法软件开发等，完成智能车工程制作及调试。大赛分光电、摄像头与电磁三个赛题组，在车模中使用透镜成像进行道路检测方法属于摄像头赛题组，检测赛道下通电漆包线磁场的属于电磁组，除此之外则属于光电赛题组。本组设计的智能小车是基于激光传感器的，属于光电赛题组。 我们的技术报告以智能小车的设计为主线，包括小车的构架设计、软硬件设计，以及控制算法研究等，分为七章。其中，第一章为摘要；第二章主要介绍了小车的作品完成功能；第三章介绍了小车的系统方案论证；第四章描述了小车的硬件结构设计及实现；第五章介绍了小车的软件结构设计及实现；第六章叙述了我们在调试小车时的测试数据；第七章写了我们组的小车的不足

4、及今后改进方向。第二章 作品完成功能图2.0 智能车整体图2.1 舵机的固定与安装 许多小组都将舵机竖着安装，因为舵机的摆臂增长可以提高舵机的响应速度。因为舵机的摆臂越长，当车轮转过相同的角度时，舵机转过的角度越小，即时间越短。但我们小组由于材料匮乏，两摆臂长不等，所以只能将舵机横着安装，并且在调试时没有太大的影响。2.2 前轮的调整调试中发现，前轮的初始状态对车子转弯有很大的影响。总的来说，决定前轮的初始状态有以下几个参数：主销内倾、主销后倾、前轮外倾、前轮前束。主销内倾角指前轮的主销轴线与地面垂直线在汽车横向断面内的夹角。内倾角可以使车轮保持直线行驶，这个角度和它的轮子回正能力是成反比的，

5、也就是说角度越大，回正能力越强，但是轮子转向阻力增大，由于这个限制，通常这个角度不会大于8度，一般3°左右就够了。当转向轮在外力作用下发生偏转时，由于主销内倾的原因，车轮连同整个汽车的前部将被抬起一定高度；当外力消失后，车轮就会在重力作用下恢复到原来的中间位置。为了避免前轮胎的磨损，我们这里将主销内倾角设置为3度左右。主销后倾角指主销轴线与车体纵向平面的夹角，它使车辆转弯时产生的离心力所形成的力矩方向与车轮偏转方向相反，迫使车轮偏转后自动回正到原来的中间位置上，从而保持汽车直线行驶的稳定性。前面所说的主销内倾角与车速是无关的，但是主销后倾角与车速以及回正力都有关。这个角度越大，回正力

6、就越大，而且速度越大，回正力也越大。但是回正力太大也不是好事，可能会损坏舵机齿轮，特别是高速时主销后倾角对回正力的影响很大，可能会损坏舵机齿轮。通常情况下，我们可以把后倾角减少到接近零度。由于过大的后倾角会使转向沉重，加上比赛使用的舵机性能偏软，故为了避免使模型车转弯迟滞，我们这里将主销后倾角设置为0度。前轮外倾角对汽车的转弯性能有直接影响，它的作用是提高前轮的转向安全性和转向操纵的轻便性。前轮外倾角俗称“外八字”，如果车轮垂直地面一旦满载就易产生变形，可能引起车轮上部向内倾侧，导致车轮联接件损坏。所以事先将车轮校偏一个外八字角度，这个角度约在1°左右。所谓前束是指两轮之间的后距离数

7、值与前距离数值之差，也指前轮中心线与纵向中心线的夹角。在日常生活中的汽车修理一般都要校对车轮前束，前轮前束的作用是保证汽车的行驶性能，减少轮胎的磨损。前轮在滚动时，其惯性力会自然将轮胎向内偏斜，如果前束适当，轮胎滚动时的偏斜方向就会抵消，轮胎内外侧磨损的现象会减少，前轮前束可通过转向横拉杆长度来调整。这里将前束值设置为6mm左右。2.3 差速的调整在车实际跑的过程中，我们发现，后面两个轮的相对松紧程度，也就是差速对拐弯有很大的影响。差速太紧，则拐弯容易甩尾，速度快的时候很容易甩出去，但是差速如果太松，虽然会改善转弯性能，但是严重影响了直线上的加减速，而且齿轮的咬合也不是很好，对齿轮会有一定的损

8、坏。所以，后面两个轮子的相对松紧程度要适中，经过多次的调试，我们得出了比较满意的效果，即将模型车放到跑到上，用手抓住后轮的一只轮子使其不能转动，在赛道上推车子转弯，如果车子能够稍轻松的推动，则此时的差速器为最适合。当然，实际还需要根据不同的赛道和车的机械性能进行相应的微调。调节差速可以通过工具旋紧或者旋松右后轮来得到合适的差速控制，其实就是适当的调整两片轴承的压力以满足后轮驱动和差速的要求。 2.4 整车重心的调整刚开始的时候由于速度比较慢，车体的重心对整辆车的影响看不出，但随着模型车速度的提高，模型车的重心对车子的整体性能的影响就显得越来越突出了。这集中表现为前轮对地摩擦力的影响和车体否能稳

9、定行驶。垂直高度上的重心影响车的稳定性，重心越低，稳定性越高，当然这要求保证车的底板不会出现触地的现象；水平方向上的重心位置及其重要，它将直接影响了前轮对地的摩擦力，也就是我们想要的前轮抓地能力的强度。重心靠前，将有利于增加前轮的摩擦力。实际测试中，稳定性对跑道S弯和急转弯的影响最大；而摩擦力则对跑道急转弯影响最重。所以为了提高车对整个跑道的综合性能，我们将车的垂直重心尽可能降到最低，而将水平重心的位置调整为靠近前轮。 图2.1 智能车布局图第三章 系统方案论证3.1 电机驱动模块方案选取 驱动电路是整个系统的重要组成部分，也是高效的算法得以实现的硬件基础。要获得一个良好的加速性能以及入弯时的

10、及时制动能力，则一个性能优异的驱动电路时不可或缺的。  对于直流电机，常用的数字调速方法就是电枢电压进行脉宽调制技术（PWM）。典型的驱动电路就是H桥式电路，可以控制电机的正反转，同时通过PWM脉宽调制可以方便的调节电机转速。3.2 H桥式电机驱动电路原理电机驱动采用H桥驱动电路，原理如下： 当Q1管和Q4管导通时，电流就从电源正极经Q1从左至右穿过电机，然后再经Q4回到电源负极。按图中电流箭头所示，该流向的电流将驱动电机顺时针转动。 当Q2管和Q3管导通时，电流就从电源正极经Q3从右至左穿过电机，然后再经Q2回到电源负极。按图中电流箭头所示，该流向的电流将驱动电机逆时针转动。3.3

11、 传感器布局由3.2所述的性质，我们可以知道，垂直线圈可以比较容易地得出小车与导线的相对位置，水平线圈可以预测前方弯道以及传感器摆放的一些要求。根据这些性质，分检测导线位置和前瞻两部分论述传感器的布局方案。3.3.1确定导线位置布局以垂直线圈作为检测小车与导线的相对位置，原则上采用双垂直线圈就可以判断导线的位置。然而增加传感器可以增加检测的精度，有利于小车的精确控制。我们选用三个垂直线圈平均间隔一字排开。3.3.2前瞻设计由于受到小车长度和垂直线圈检测方式限制，电磁小车的前瞻受到很大的局限。尽管小车长度达到了极限，而小车的前瞻却只有10cm左右，不能满足智能车高速运行的要求。因此需要用必要的手

12、段增加小车的前瞻性能。方案一：双排传感器。单排传感器检测信息单一，而双排传感器可以通过判断导线斜率来弥补前瞻不足。由于双排传感器检测的信息较为丰富，可以合理利用其信息作转角以及速度的控制。测试发现的确优于单排传感器。方案二：合理变更传感器方向。由于水平线圈对远方道路比较敏感，可以感知道路的变化趋势，因此可以利用这个特点进行前瞻。以6电感线圈为例，这种方法的前瞻可以在原来传感器位置的基础上前瞻约10cm的路况。变更其摆放的方向和角度，可以在传感器原位置基础上前瞻约15cm25cm的距离。这样，小车的前瞻最大可达到约35cm。如果检测线圈直径较大，则可以做到约40cm的前瞻，下称“大前瞻”。我们采

13、用与小车前进方向一致的摆法。这样的前瞻在5cm以内，下称“弱前瞻”。由于线圈放置方向的关系，前瞻重要解决的问题是能否顺利通过十字交叉的赛道。导线十字交叉出现场强叠加的情况，这样对大前瞻来说是一个严重的干扰。尽管经过处理之后小车能够顺利通过十字交叉路口，但无可避免会有少许抖动。这样既影响小车稳定性，也使行进速度有一定下降。而弱前瞻具有一定的前瞻，尽管非常小，但总比没有前瞻要好。在十字交叉处几乎不受干扰。权衡利弊，从小车的稳定性考虑，选择了弱前瞻的方案。第四章 硬件电路模块 整个硬件系统使用模块化的设计思想，整体的硬件框图如图4所示： 电源模块S12XS128控制核心单片机LCD液晶显示与键盘模块

14、控制器模块起跑线检测模块测速模块舵机使能控制电路模块电机驱动模块 图4 系统硬件结构图 4.1 控制器模块小车控制器是大赛指定的XS128芯片。使用BDM下载。经过多次测试和在跑道上跑，信号采集只需要7路AD检测，同时需要的I/O资源也不多，因而选用80pin的芯片则满足要求。而且80pin的CPU价格比多管脚的CPU便宜，功能上差不多，只是AD口少了一半，其他没有什么区别。单片机控制器是各功能模块的CPU，在满足智能车功能的前提下，我们本着最简单电路的原则，使用了单片机的若干端口，单片机端口分配图如表4.1所示表4.1 端口资源分配AD模块PAD0-PAD6传感器信号检测PWM模块PP1舵机

15、控制PP3电机控制ECT模块PT7测速（编码器脉冲输入）EXT模块PE1起跑线检测I/0口PE6、PB1、PB3、PB5、PB7液晶显示PA1、PA3、PA5、PA7按键PB0舵机使能控制4.2 路径识别模块测量磁场核心是检测线圈的感应电动势E的幅值。信号检测放大电路包括感应线圈、信号选频放大、整流三部分组成。4.2.1感应线圈检测线圈可以自行绕制，也可以使用市场上能够比较方便购买的工字型10mH的电感。这类电感体积小，Q值高，具有开放的磁芯，可以感应周围交变的磁场。4.2.2信号选频放大使用电感线圈可以对其周围的交变磁场感应出感应电动势。这个感应电动势信号比较弱，干扰多。因此信号放大需要进行

16、选频放大，使得20kHz的信号能够有效的放大，并且去除其它干扰信号的影响。可以使用 LC串并联谐振电路（带通电路）来实现选频电路。如图4.2.1所示：图4.2.1 选频电路图电路谐振频率为： （公式7）已知感应电动势的频率f0=20KHz，感应线圈电感为L=10mH，由（公式7）可以得到电容容量的表达式如（公式8）。 （公式8）由（公式8）可以计算出谐振电容的容量为：通常在市场上可以购买到的标称电容与上述容值最为接近的电容为 6.8nF，所以在实际电路中我们选用 6.8nF的电容作为谐振电容。为了能够更加准确测量感应电容式的电压，还需要将上述感应电压进一步放大，一般情况下将电压峰峰值放大到15

17、V左右，就可以进行幅度检测，所以需要放大电路具有100倍左右的电压增益（40db）。最简单的设计可以只是用一阶共射三极管放大电路就可以满足要求。也可以选用运算放大器进行电压放大。但是需要选择低噪音、动态范围大的高速运放，成本较高，所示不选用运算放大器进行信号放大。一般晶体三极管带宽较大，因而大体只需考虑放大倍数。可以选取常用的NPN三极管8050-D或1815-GR，我们选取的是1815-GR作为放大。4.2.3检波整流测量放大后的感应电动势的幅值E使用二极管检波电路将交变的电压信号检波形成直流信号，该信号正比于感应电压幅值的数值，可以知道单片机的AD进行测量。为了能够获得更大的动态范围，检波

18、电路中的二极管使用肖特基二极管1N5819。由于肖特基二极管的开启电压一般在0.10.3V左右，小于普通的硅二极管（0.7V），可以增加输出信号的动态范围和增加整体电路的灵敏度。最终，我们得到整个路径识别模块的电路图如图4.2.2所示：图4.2.2 信号放大处理电路图4.3 电源模块智能小车采用组委会提供的7.2V 2000mAh Ni-cd 电池直接供电，并采用稳压器件调整出各路模块所需电源。电路结构如图4.3.1所示。图4.3.1 电源模块结构图4.4 舵机使能控制电路可以说，舵机是一个非常脆弱的部件，稍有不慎就会烧掉，特别是被卡住的时候，为了防止在下载程序的时候舵机处于供电状态，所以加了

19、一个舵机使能控制电路，舵机使能电路相当于一个软开关，可以通过软件去控制舵机电源。设置PB0口可以使得舵机只有在程序被下进去并且运行之后才会被供电，否则处于断电状态，原理图如图4.5.1所示。图4.5.1 舵机使能控制电路4.5 电机驱动模块为了加强小车后轮驱动能力，采用大功率MOS管IRF4905和IRF3205搭成H桥驱动电机。为使小车能够快速的加减速，可以采用反转进行刹车。但电机的正反转会产生很大的制动电流，对电机驱动本身造成很大的损害。而在实际应用中只使用到半桥，其中一个控制端接地。当需要制动时，只要使电机对地短接，依靠电机反电动势制动同样得到较好的制动效果。驱动和单片机之间使用了MIC

20、4424驱动H桥电路，既保证了开启MOS管栅极的逻辑控制，也对单片机有隔离保护的作用。 图4.6.1 电机驱动电路图 第五章 智能车软件设计5.1 控制总流程本队所用的软件调试工具为CodeWarrior软件和BDM仿真器，这个软件支持C语言和汇编语言的混合编程，由于C语言操作简单，可修改和移植性强，所以大部分程序都使用C语言编写，只有在某些地方加入了汇编语句。编程基本上采用模块化的思想，使得整个设计清晰明了。下面是系统总体的软程图。 图5.1 系统软件流程图5.2 导线位置提取方案一：数字检测法。把检测回来的信号数字化，根据数字量判断导线的位置。这种方法路径分辨率不高，而且当导线电流不稳定的

21、时候，容易受到干扰，适应性不强。为提高空间分辨率，只能增加传感器数量，但每个传感器之间相互会造成较大的干扰。方案二：模拟检测法。根据采集回来的ad值进行合理的运算，判断导线的位置。这种方法空间分辨率可以达到2mm，而且受电流变化的影响比较少，适合小车稳定的检测要求。故选方案二作为导线提取方法。首先，将 AD值做归一化处理，即根据各个传感器接收赛道的最高电压和最低电压，计算出各个传感器的相对值，最后来计算黑线位置。信号归一化的方法如下：求取电压值最大的传感器位置，然后和它周围两个传感器采样值进行加权计算即可求得小车的偏差。5.3 系统控制算法提取小车和导线之间的位置关系后，可以确定基本控制策略。

22、当小车偏离导线较少时，给定一个较小的回正角度，小车也可以加速行驶。当小车偏离导线较大时，应该给定较大的回正角度和减慢车速。5.3.1舵机转角控制算法void Servo_control() float SP=2; AD_jzlb(); ad_chazhi=ad_lb2-ad_lb0+3;/为调整直道时车在中间加3 error_ad_chazhi=ad_chazhi-pre_ad_chazhi; if(!left_flag&&!right_flag) if(ad_lb0<5)left_flag=1; if(ad_lb2<5)right_flag=1; if(ad_lb

23、1>45)SP=2; else if(ad_lb1>35)SP=5; else if(ad_lb1>20)SP=8; else SP=35; ServoPWM=mid_ServoPWM+SP*ad_chazhi-2*error_ad_chazhi;/-2;/调试ad_chazhi和error_ad_chazhi的系数? /ad_chazhi_square=ad_chazhi2; /ServoPWM=1600+15*ad_chazhi_square-2*error_ad_chazhi;/-2;/调试ad_chazhi_square和error_ad_chazhi的系数? if(

24、left_flag=1) right_flag=0; if(ad_lb1>38)left_flag=0; ServoPWM=mid_ServoPWM+7*(50-ad_lb0)+2*error_ad_chazhi;if(right_flag=1) left_flag=0; if(ad_lb1>38)right_flag=0; ServoPWM=mid_ServoPWM-7*(50-ad_lb2)-2*error_ad_chazhi; 5.3.2电机转速控制算法 void Motor_control() AD_jzlb(); if(ad_lb1>=40) /MortorPWM+

25、; MortorPWM=MortorPWM+5; if(MortorPWM>max_MortorPWM)MortorPWM=max_MortorPWM; else if(ad_lb1>=10) MortorPWM=MortorPWM-5; if(MortorPWM<wan_MortorPWM)MortorPWM=wan_MortorPWM; if(ad_lb0<1&&ad_lb1<1&&ad_lb2<1) /Motor_left_brake(); /Motor_right_brake(); MortorPWM=0; Motor

26、_left_forward(MortorPWM); Motor_right_forward(MortorPWM); 第六章 作品测试数据1.舵机转动函数 左边最大值1220 ,右边最大值1940,中间值1600。2.FLAG（5，5，38），SP(2,5,8,35),+3,(7,2,7,2)V:6090,wandao_V:45/40,减速阈值40，可以 15S（较稳定） FLAG（5，5，38），SP(2,5,8,35),+3,(7,2,7,2)V:60100,wandao_V:50,减速阈值40，可以 15S（较稳定）。 FLAG（5，5，38），SP(2,5,8,35),+3,(7,2,7

27、,2)V:6090,wandao_V:45/40,减速阈值40，可以 15S（较稳定） FLAG（5，5，38），SP(2,5,8,35),+3,(7,2,7,2)V:60100,wandao_V:50,减速阈值40，可以 15S（较稳定） FLAG（5，5，38），SP(2,5,8,35),+3,(7,2,7,2)V:70100,wandao_V:60,减速阈值40，可以 14S（较稳定）3. 为调整直道时车在中间，在程序段中ad_chazhi=ad_lb2-ad_lb0+3;/为调整直道时车在中间加3。 第七章 不足及今后改进方向 经过几个月时间的筹备，我队顺利完成了智能车的设计与制作，实

28、现了智能车的寻迹自动行驶功能。在此过程中，我们完成了小车的组装，程序的编写以及小车的调试。 由于知识水平和制作条件的限制，智能车在行驶过程中还存在一些问题。1.车子安装的稳定性问题。由于材料的限制，我们组小车的固定的强度不是太好，在小车调试的时候，舵机上的传动装置和测速用的编码盘，还有前瞻都掉下来过。2.本智能车系统在控制算法上还是采用传统的控制算法，在智能算法研究上还有所欠缺。因此，在之后的工作中，智能算法的研究将是控制算法改进的主要方向之一。附录A 部分程序代码(带 / 或 /* */ 符号的为注释)#include <hidef.h> /* common defines an

29、d macros */#include "derivative.h" /* derivative-specific definitions */PWM常量定义/#define CH_SA_DIV 4#define BUS_FREQ 8000000 #define CH_A_PREQ 8000000#define CH_SA_PREQ 1000000#define CH_B_PREQ 8000000#define CH_SB_PREQ 1000000#define Servo_PREQ 50#define Motor_PREQ 5000#define Abs(x) (x)&g

30、t;0)?(x):(-(x) /定义绝对值函数/变量定义volatile int advalue45=0,ad_lb5 = 0; /AD数据采样，ad_lbi是加权递推均值滤波后的采样值,ad_standard是进一步归一化后的值volatile int ad_chazhi,ad_chazhi_square,error_ad_chazhi,pre_ad_chazhi=10; /变量分别为：AD差值，当前差值误差，前一次差值误差volatile int ServoPWM,mid_ServoPWM=1750;volatile unsigned char MortorPWM,wan_MortorPW

31、M,max_MortorPWM;volatile int left_flag=0,right_flag=0;/* Function:AD采样模块*/void AD_init(void) ATD0DIEN=0x00; /禁止数字输入 ATD0CTL0=0x04; /转换AD0AD3 ATD0CTL1=0x00; /7:非外部触发,65:00-8位精度（10-12位精度）,4:不放电,3210:ch ATD0CTL2=0x40; /禁止外部触发, 中断禁止 ATD0CTL3=0xc0; /右对齐无符号,每次转换8个序列, No FIFO, Freeze模式下继续转 ATD0CTL4=0x01; /

32、765:采样时间为4个AD时钟周期,ATDClock=BusClock*0.5/PRS+1 ATD0CTL5=0x30; /6:0特殊通道禁止,5:1连续转换 ,4:1多通道轮流采样 /-ADC转换部分-void getvalue() while(!ATD0STAT2L_CCF0); advalue00 = ATD0DR0; while(!ATD0STAT2L_CCF1); advalue10 = ATD0DR1; while(!ATD0STAT2L_CCF2); advalue20 = ATD0DR2; while(!ATD0STAT2L_CCF2); advalue30 = ATD0DR3

33、; /* Funciton:加权递推平均滤波法 本函数是3电感滤波，队列长度为5 */void AD_jzlb(void) int i,j; for(i=0;i<4;i+) for(j=3;j>=0;j-) advalueij+1 = advalueij; getvalue(); for(i=0;i<5;i+) ad_lbi = 45*advaluei0/75 + 16*advaluei1/75 + 8*advaluei2/75 + 4*advaluei3/75 + 2*advaluei4/75; /ad_lbi 0.75 i越小，权值越重，因为i越小时 /延时函数，延时1m

34、s void delay(unsigned int ms) unsigned int i,j; for(j=0;j<ms;j+) for(i=0;i<1325;i+); void PWM_init1(void) /电机PWM初始化 PWME=0x00; PWMCLK=0xc3; PWMPRCLK=0x00; PWMSCLA=4; PWMPER0=CH_SA_PREQ/Motor_PREQ; PWMPER1=CH_SA_PREQ/Motor_PREQ; PWMSCLB=4; PWMPER6=CH_SB_PREQ/Motor_PREQ; PWMPER7=CH_SB_PREQ/Motor

35、_PREQ; PWMCTL=0x00;void PWM_init2(void) /舵机PWM初始化 PWME_PWME3=0; PWMCLK_PCLK3=1; PWMPRCLK=0x00; PWMSCLB=4; PWMCTL_CON23=1; PWMPER23=CH_SB_PREQ/Servo_PREQ; PWMPOL=0x4a; PWMCAE=0x00; PWME_PWME3=1;void Servo(unsigned int duty) /舵机转动函数 左边最大值1220 ,右边最大值1940,中间值1600 PWMDTY23=duty;void Motor_right_forward(u

36、nsigned char duty) /右电机前转函数 PWME_PWME0=0; PWME_PWME1=1; DDRK_DDRK4=1; PORTK_PK4=1; PWMDTY1=duty;void Motor_right_brake(void) /右电机停转函数 PWME_PWME0=0; PWME_PWME1=0; DDRK_DDRK5=1; PORTK_PK5=0;void Motor_left_forward(unsigned char duty) / 左电机前转函数 PWME_PWME6=1; PWME_PWME7=0; DDRK_DDRK5=1; PORTK_PK5=1; PWM

37、DTY6=duty;void Motor_left_brake(void) / 左电机停转函数 PWME_PWME6=0; PWME_PWME7=0; DDRK_DDRK4=1; PORTK_PK4=0;/电机控制 void Motor_control() AD_jzlb(); if(ad_lb1>=40) /MortorPWM+; MortorPWM=MortorPWM+5; if(MortorPWM>max_MortorPWM)MortorPWM=max_MortorPWM; else if(ad_lb1>=10) MortorPWM=MortorPWM-5; if(Mo

38、rtorPWM<wan_MortorPWM)MortorPWM=wan_MortorPWM; if(ad_lb0<1&&ad_lb1<1&&ad_lb2<1) /Motor_left_brake(); /Motor_right_brake(); MortorPWM=0; Motor_left_forward(MortorPWM); Motor_right_forward(MortorPWM); /舵机控制 void Servo_control() float SP=2; AD_jzlb(); ad_chazhi=ad_lb2-ad_lb0

39、+3;/为调整直道时车在中间加3 error_ad_chazhi=ad_chazhi-pre_ad_chazhi; if(!left_flag&&!right_flag) if(ad_lb0<5)left_flag=1; if(ad_lb2<5)right_flag=1; if(ad_lb1>45)SP=2; else if(ad_lb1>35)SP=5; else if(ad_lb1>20)SP=8; else SP=35; ServoPWM=mid_ServoPWM+SP*ad_chazhi-2*error_ad_chazhi;/-2;/调试ad_c