基于磁导航路径识别的智能车设计

工学院本科生毕业设计（论文）基于磁导航路径识别的智能车设计软件设计The Design of Smart Car Based on Magnetic Navigation Path IdentificationSoftware Design总 计： 37 页表 格： 1 个插 图 ： 16 幅基于磁导航路径识别的智能车设计软件设计I基于磁导航路径识别的智能车设计软件设计自动化专业 摘 要 本设计以飞思卡尔 16 位单片机 MC9S12XS128 作为核心控制器，在智能车控制系统硬件设计的基础上，通过使用软件 Freescale CodeWarrior 对单片机编写控制程序，实现智能车的自动循迹。设计中利用电磁传感器采集道路磁场信息，通过信号采集、路径识别等算法，判断智能车的运行状态和路径形状；利用测速编码器进行速度检测，然后通过角度控制、速度控制等算法，确定对电机和舵机的控制；最后结合硬件系统实现智能车速度和转向的自动控制，使智能车能够快速、稳定的跑完赛道。关键词 磁导航；路径识别；角度控制；速度控制 基于磁导航路径识别的智能车设计软件设计IIThe Design of Smart Car Based on Magnetic Navigation Path IdentificationSoftware DesignAutomation Specialty Abstract: The design is based on the hardware design of intelligent vehicle control system whose core controller is Freescales 16-bit microcontroller MC9S12XS128, and the intelligent vehicle achieves automatic tracking through executing control procedures by the use of Freescale CodeWarrior. The software design collects the magnetic information on the road by using electromagnetic field sensors, judges the operational status of intelligent vehicle and path shape through the signal acquisition and path recognition algorithm, detects speed through the use of speed detection encoder, then determines the control of the motor and steering gear through angle control and speed control algorithm, and finally achieves the automatic controller of speed and steering of the intelligent vehicle by integrated with the hardware systems, so the intelligent vehicle can finish the track quickly and stability.Key words: Magnetic navigation; path identification; angle control; speed control基于磁导航路径识别的智能车设计软件设计III目 录1 引言 .11.1 课题背景 .11.1.1 国外智能车研究概况 .11.1.2 国内智能车研究概况 .21.1.3 应用前景 .21.2 本课题主要研究内容 .32 系统整体设计 .32.1 磁导航原理 .32.2 系统设计分析 .52.2.1 硬件设计分析 .52.2.2 软件设计分析 .73 软件设计 .83.1 传感器信号采集 .83.2 智能车偏移、转向和路径形状判断 .93.2.1 智能车偏移 .93.2.2 智能车转向 .103.2.3 路径形状 .103.3 角度控制算法 .103.3.1 十字路口 .113.3.2 其它路径 .123.4 速度控制算法 .133.4.1 十字路口 .133.4.2 其它路径 .133.5 智能车的启动与停止 .134 编程与调试.144.1 Freescale CodeWarrior V5.9 软件及其应用 .144.2 编程思路 .174.2.1 主程序 .174.2.2 中断子程序 .174.3 系统调试 .18基于磁导航路径识别的智能车设计软件设计IV4.3.1 弯道 .184.3.2 十字路口 .194.3.3 其它 .19结束语 .20参考文献 .21附录 .22致谢 .31基于磁导航路径识别的智能车设计软件设计11 引言1.1 课题背景十九世纪末，随着内燃机的诞生，人们发明了最现代化的交通工具汽车。经过一个多世纪的发展，汽车技术、性能有了很大的提高，人们充分享受到了汽车带来的巨大便利。但是，在享受汽车带来便利的同时，人们也发现汽车也给社会的发展带来了不少的损失，甚至危害到了人们的人身安全。由于公路客、货运输量的迅速增长，人们深受交通拥挤、堵塞严重、事故频繁和环境污染等公害的困挠。尤其是随着高速公路发展，汽车速度的提高，各类恶性交通事故的发生呈不断上升趋势，给人们的生命财产造成了巨大的损失。这迫使人们采用高新技术以提高车辆的安全性、可靠性，以解决道路交通的公害问题。智能车辆的研究发展正是在这种背景下诞生的。1.1.1 国外智能车研究概况美国国家科学委员会曾预言：“20世纪的核心武器是坦克，21世纪的核心武器是无人作战系统，其中2000年以后遥控地面无人作战系统将连续装备部队，并走向战场。”为此，从80年代开始美国国防高级研究计划局(DARPA)专门立项，制定了地面无人作战平台的战略计划，目标是研制出满足战场需要的智能车辆，可以在崎岖的地形上沿规划的路线自主导航及躲避障碍，必要时重新规划其路线。从此，在全世界掀开了全面研究智能车辆的序幕，如DARPA的“战略计算机”、计划中的自主地面车辆(ALV)计划(1983-1990)、能源部制订的为期10年的机器人和智能系统计划(RIPS)(1986-1995)、以及后来的空间机器人计划等等，另外，日本通产省组织的极限环境下作业的机器人计划、欧洲尤里卡中的移动机器人计划等。虽然智能车辆的研究起源于军事的要求，但是在其他领域的应用也有极大的价值，并且在研究上也取得了一定的成果。在太空探索方面，美国NASA研制的火星探测机器人索杰那于1997年成功登上火星，这是一个具有六个轮子的自主移动机器人。为了在火星上进行长距离探险，NASA又开始了新一代样机的研制，命名为Rocky7，并在Lavic湖的岩溶流上和干枯的湖床上进行了成功的实验。在民用方面，智能车辆也有许多成功的例子。如日本的VERTIS智能汽车系统，该智能汽车主要有23个ITSZ子系统，主要用于实现车载通讯、信息加工处理、环境探测、辅助控制(自动驾驶)等四项功能。法国公路技术研究所技术顾问克洛德科贝表示，开展自动行驶控制系统研究是迈向公路交通无人驾驶的第一步，这一系统研制成功后，可以首先实现有人自动行驶。如在超车时，驾驶员只需根据计算机所提示的前方车辆行驶速度，输入有关最高限速等简单命令，计算机便会根据公路上的具体情况，基于磁导航路径识别的智能车设计软件设计2自动调整速度和方向，并在绝对安全的情况下，实现超车。这将大大减少由驾驶员的判断和操作失误导致的交通事故。1.1.2 国内智能车研究概况虽然我国对智能车辆的研究起步较晚，但是发展较快。1986年开始的高技术研究发展计划(863计划)制定了智能机器人主题的总体战略目标。1994年10月清华大学研制成我国第一台室外智能移动机器人THMR-III。之后，国防科技大学，北京理工大学，南京理工大学陆续开展了智能车辆的研究。我国清华大学计算机科学与技术系的人工智能与智能控制实验室已进行了多年的研究，并且取得了很大的成果。目前，他们研制的自主移动机器人实验车THMR-V已经能够在较复杂的环境自行行驶，他们在这个系统中采用激光雷达和摄像机来做视觉系统，并采用自己已有的先进的图像处理技术，进行信息识别和处理，再进行智能控制实现汽车的运动。这就是我国目前最先进的智能自动驾驶汽车系统。为了更好的实现各项功能，他们正在进行进一步的研究和完善，使其在实际生活中得以应用。1.1.3 应用前景城市公共交通是与人民群众生产生活息息相关的重要基础设施。然而，目前世界上许多大城市都面临着由私人汽车过度使用而带来的诸多问题，例如道路堵塞、停车困难、能源消耗、噪声污染和环境污染等，这些问题严重降低了城市生活的质量。优先发展城市公共交通是提高交通资源利用效率，缓解交通拥堵的重要手段。大容量城市公共交通，如地铁、轻轨等，其最大优点是空间利用率和能源利用率较高。然而，由于缺乏足够的时间、空间、运力灵活性，在客流量不足的情况下，系统效率将大大降低，运营成本过高，难以大力推广和应用。回顾汽车发展的百年历史，不难发现其控制方式从未发生过根本性改变，即由人观察道路并驾驶车辆，形成“路人车”的闭环交通系统。随着交通需求的增加，这种传统车辆控制方式的局限性日益明显，例如安全性低（交通事故）和效率低（交通堵塞）。最新调查表明，95的交通事故是由人为因素造成，交通堵塞也大都与驾驶员不严格遵守交通规则有关。如果要从根本上解决这一问题，就需要将“人”从交通控制系统中请出来，形成“车路”闭环交通系统，从而提高安全性和系统效率。这种新型车辆控制方法的核心，就是实现车辆的智能化。智能车有极为广泛的应用前景。结合传感器技术和自动驾驶技术可以实现汽车的自适应巡航并把车开得又快又稳、安全可靠；汽车夜间行驶时，如果装上红外摄像头，就能实现夜晚汽车的安全辅助驾驶；也可以工作在仓库、码头、工厂或危险、有毒、基于磁导航路径识别的智能车设计软件设计3有害的工作环境里，此外它还能担当起无人值守的巡逻监视、物料的运输、消防灭火等任务。在普通家庭轿车消费中，智能车的研发也是很有价值的，比如雾天能见度差，人工驾驶经常发生碰撞，如果用上这种设备，激光雷达会自动探测前方的障碍物，电脑会控制车辆自动停下来，撞车就不会发生 1。1.2 本课题主要研究内容本论文所研究的智能车，是基于嵌入式单片机技术和传感器技术的自动寻迹智能车。其基本功能是通过检测电磁信号来判断路径形状如直道、弯道、十字路口等，并进行相应的动作如拐弯、加速、减速等。本文对磁导航的原理进行了详细分析，简单的介绍了硬件部分，重点分析和设计了路径识别算法、角度控制算法、速度控制算法，编写了控制程序 2。2 系统整体设计基于磁导航路径识别的智能车控制系统要求智能车能够通过感应道路中心导线流过的交变电流产生的磁场进行路径检测，识别智能车的运行状态和路径形状。智能车用电磁传感器来采集磁场信号并将之转化为电压信号，然后将电压信号传送给单片机，经过单片机分析、处理后，实现智能车位置的判别，并控制电机和舵机采取相应的动作，实现智能车的自动循迹行驶。本章详细介绍了智能车系统的整体结构及相应的设计分析。2.1 磁导航原理根据麦克斯韦电磁场理论，交变电流会在周围产生交变的电磁场，导线周围的电场和磁场，按照一定规律分布，如图1所示。基于磁导航路径识别的智能车设计软件设计4图1 通电导线周围磁场分布图通电导线周围感应磁场的分布是以导线为轴的一系列的同心圆。圆上的磁场强度大小相同，并随着距离导线的半径 r 增加成反比下降。由毕奥-萨伐尔定律知，距离导线距离为 r 处 P 点的磁感应强度如公式1所示：（1）=04通电导线周围的磁场是一个矢量场，场的分布如图四所示。如果在通电直导线两边的周围竖直放置两个轴线相互垂直并位于与导线相垂直平面内的线圈，则可以感应磁场向量的两个垂直分量，进而可以获得磁场的强度和方向，如图2所示。图2 线圈感应磁场图导线中的电流按一定规律变化时，导线周围的磁场也将发生变化，则线圈中将感应出一定的电动势。根据法拉第定律，线圈磁场传感器的内部感应电压 E 与磁场 B、电磁线圈的圈数 N 、截面积 A的关系如公式2所示：基于磁导航路径识别的智能车设计软件设计5（2）=()(0)=感应电动势的方向可以用楞次定律来确定。由于本设计中导线中通过的电流频率较低，为20kHz，且线圈较小，令线圈中心到导线的距离为 r ，认为小范围内磁场分布是均匀的。再根据图3 所示的导线周围磁场分布规律，则线圈中感应电动势如公式3所示：（3）=即线圈中感应电动势的大小正比于电流的变化率，反比于线圈中心到导线的距离，其中常量 K 为与线圈摆放方法、线圈面积和一些物理常量有关的一个量 3。根据上述原理，在智能车上适当位置安装若干个线圈，通过检测固定于智能车上确定位置的若干个线圈产生的感应电动势，将这些模拟值放大、滤波后交给单片机以进行转化、处理、比较，就可以确定智能车相对于道路中心的偏移、正在进行的转向和路径形状，以决定采取何种控制策略来使智能车自动循迹行驶。2.2 系统设计分析本设计要求制作一个能够自主识别路径的智能车，在专门设计的跑道上自动识别路径行驶，并且最快跑完全程而没有冲出跑道。智能车要求跑的又快又稳，所以对于智能车的控制系统来说稳定性和快速性是控制系统设计的两个重要指标。智能车所需要的电磁信号由道路中心线处通以100mA交变电流的导线产生，智能车通过设置在前端的6个线圈感应磁场来获取位置信息，通过安装在后轮上的测速编码器获取速度信息。位置信息和速度信息送入MCU进行处理和运算，然后由MCU输出信号控制舵机的偏转和电机的转速，以实现小车的转向和速度控制 4。智能车控制系统的整体结构如图 3 所示。基于磁导航路径识别的智能车设计软件设计6MC9S12XS128各传感器及其信号处理电路舵机控制转向电源管理模块电池电机驱动电路图 3 智能车控制系统整体结构图2.2.1 硬件设计分析智能车控制系统从硬件上分为传感器模块、电机驱动模块、舵机驱动模块和单片机模块等。（1）传感器模块本智能车控制系统主要由传感器进行导航，因此传感器的灵敏度对智能车的稳定、快速行驶非常重要 5。传感器电路如图 4 所示。图 4 传感器电路图电路中的 L1 和 C1 构成 LC 选频电路，选择 L1=100mH，C1=6.8nf；电阻基于磁导航路径识别的智能车设计软件设计7RV1、R4、R2 构成分压电路，通过调节 RV1 的值来改变 R2 两端的电压，从而改变三极管 C1815 的基极和发射极之间的电压，以此来调节三极管的静态工作点；电容 C3,C4用来滤波；C3 之后的电路为倍压检波电路，将交流信号转换为直流信号；最后输出值约等于交流信号的峰值 6。实物如图 5 所示（图中最左端的两个引脚接电感） 。图 5 传感器实物图（2）电机驱动模块电机驱动电路采用 MC33886 并联控制电路，使用时直接将两个 MC33886 叠加起来就可以了。（3）舵机模块实验发现，由于车模提供的舵机连杆的长度不一样，左右转向都能很好的适应转弯的需要很难达到，故使用等长的铜柱来代替；且舵机竖直安装，这样力臂等长，对左右转向影响差别不大，很好的适应需要，使左转与右转同样顺利。（4）单片机模块单片机模块插在主控板上，各个端口如下面的电路图所示。这个主控板中舵机电源为 5V，主控板中的除舵机和电机之外的电源电路采用 7805 芯片稳压芯片从电池转换而来。根据实际情况要求，所有电容容量总和不能超过 2000uf，每个电容的耐压值不能超过 25V。考虑到智能车的跟随性和灵敏性，主控板不能用感光板制作，因为感光板太重了，尽量选质量最轻的材料来做，并减少焊锡的使用量，总之就是想尽办法将小车的重量减下来。由于用于控制的 PWM 波的频率为 8kHz，频率太高了，光电隔离反应不过来，所以在影响不大的情况先不建议在电机的 PWM 端口加光电隔离，但舵机的可以加 7。主控板电路图如图 6 所示。基于磁导航路径识别的智能车设计软件设计8图 6 主控板2.2.2 软件设计分析软件整体结构如图 7 所示。单片机初始化信号采集信号处理舵机转向控制 电机驱动轮速信号基于磁导航路径识别的智能车设计软件设计9图 7 软件结构图单片机的初始化：包括 I/O 口的初始化、时钟的初始化、PWM 模块的初始化、ATD 模块的初始化、中断的初始化。信号的采集：间隔固定时间，采集传感器输入的信号。信号的处理：将采集到的信号存储在单片机中，通过对信号的分析和计算，判断车体相对于道路中心线的偏移、车体正在进行的转向和路径形状，从而进一步的控制舵机的转角和驱动电机的转速。舵机转角的控制和电机转速的控制：通过控制 PWM 波输出可以实现对舵机转角和轮速的控制。轮速传感信号：通过测速编码器将转速转化为电信号输入 8。3 软件设计设计完硬件系统后，接下来设计软件，本设计软件系统主要有以下几部分：路径识别算法、角度控制算法、速度控制算法等。控制策略的选择对于智能车的行驶性能是非常重要的，控制智能车的最终目的就是要使智能车在平稳行驶的前提下，尽可能地以最快的速度和最短的路线行驶，因而可将软件控制大体上的划分为路径识别、速度控制和角度控制三大模块 9。3.1 传感器信号采集传感器安装如图8所示，图中H0、H1、H2、H3为竖直线圈，L0、L1为水平线圈。基于磁导航路径识别的智能车设计软件设计10图8 传感器安装图为保存采样的模拟值，设置两个数组ADvalue610和AD6。采样时，连续采样10次，将H0、H1、H2、H3、L0、L1的值分别赋给ADvalue0i、ADvalue1i、ADvalue2i、ADvalue3i、ADvalue4i、 ADvalue5i；然后去掉两个最小值、两个最大值，取剩下六个值的平均值，再分别赋给AD0、AD1、AD2、AD3、AD4、AD5，作为H0、H1、H2、H3、L0、L1的值。采样的数值必须实时反应智能车的偏移、转向和路径形状，因此采样频率不能太低；采样频率过高，单片机计算负荷过大，也不合适。经过实验，确定每500 s采样一次，可以快速并准确的反应上述指标 10。3.2 智能车偏移、转向和路径形状判断智能车要实现自动循迹行驶，必须要通过对传感器信号的分析，确定智能车的偏移、转向和路径形状。通过判断智能车的左偏或者右偏，来决定智能车是右转还是左转；通过判断智能车的转向来决定角度控制是否加上微分；通过路径判断来确定采用何种角度控制策略。3.2.1 智能车偏移主要用竖直传感器H2、H3来判断智能车相对道路中心位置的偏移。若AD3AD2，说明竖直传感器H3比H2更靠近道路中心，也即车体左偏，智能车需要右转；若AD3150) AD2=AD0;if(AD2-AD0150) AD0=AD2;/ H0与H2互补盲区if(AD1-AD3150) AD3=AD1;if(AD3-AD1150) AD1=AD3;/ H1与H3互补盲区3.2.2 智能车转向判断智能车的转向需要先确定车体是左偏还是右偏，然后用远离道路中心的那一个水平传感器来判断智能车的转向。这里定义六个整形变量来辅助判断：水平传感器L0当前值analog0和前一次值b_analog0，水平传感器L1当前值analog1和前一次值b_analog1，L0变化analog0_ec，L1变化analog1_ec。其中，analog0_ec= analog0- b_analog0；analog1_ec= analog1- b_analog1。假如已经确定了智能车左偏，那么用远离道路中心的水平传感器L0的值来判断车体的转向：如果analog0_ec0，即L0的值变大，说明L0在接近道路中心，即智能车正在右转。3.2.3 路径形状（1）十字路口：根据右手定则，十字路口由于磁场叠加，竖直传感器已经判断出的方向正好与实际的相反，而水平传感器却几乎不受影响，因此当水平传感器 L0、L1的方向与竖直传感器 H2、H3 判断的方向相反，而且相反的有点大，说明智能车进入十字路口。也即（AD4-AD5=250)&(AD3-AD2200)(AD5-AD4=250)&(AD2-AD3200）有一个成立的话，认为进入路十字路口。（2）弯道：如果 AD4和 AD5中至少一个小于 100，则认为进入弯道。3.3 角度控制算法 十字路口由于磁场叠加，如果智能车保持之前的角度控制策略，容易不稳定。此处使智能车进入十字路口保持恒定转角控制一段时间后，恢复角度随动控制，并给角基于磁导航路径识别的智能车设计软件设计12度控制加上微分控制。其它路径形状，采用角度比例控制，只需根据直弯道情况调整角度比例控制系数 11。3.3.1 十字路口 进入十字路口，由于磁场叠加，竖直传感器不能正确的判断方向，因此不能采用普通路径的角度控制算法。进入十字路口后需要保持恒定转角一段时间，在这个时间内智能车角度控制由两个水平传感器的差值来决定。此处需要将两个水平传感器采集的对称位置的数值尽量调成一致，十字路口角度的方向由 AD4与 AD5的差值来判断，大小等于角度比例控制系数*（|AD4-AD5|） ；之后采用角度比例微分控制，智能车离开十字路口后，恢复角度比例控制。经过调试，智能车通过十字路口需要 80ms 的时间，其中恒定转角控制时间 30ms，角度比例微分控制时间 50ms。程序中，首先定义两个变量 cross 和 cross2，分别作为智能车进入十字路口标志位和进入十字路口保持标志位。如果判断智能车进入十字路口，令 cross=1，cross2=1。30ms 后，令十字路口保持标志位 cross2=0，采用角度比例微分控制。50ms 后，令 cross=0，智能车离开十字路口。十字路口角度控制流程图如图 9 所示。进入十字路口保持时间到 ?离开十字路口 ?是否是否其它路径角度控制算法恒定角度控制角度 P D 控制图 9 十字路口角度控制流程图程序段如下：if(cross=1) count1+;基于磁导航路径识别的智能车设计软件设计13if(count13000) cross2=0;if(count1=8000) cross=0;count1=0;其中 count1 为计数控制字，用于计算保持时间和十字路口行驶时间。3.3.2 其它路径如果判断智能车进入弯道，并且在向减小偏移的方向转向，则需要将角度比例系数增大；如果判断智能车进入弯道并且在向增大偏移的方向转向，则需要将角度比例系数增大同时加入角度微分控制。此处以智能车左偏（AD3AD2）为例，说明其它路径的角度控制算法，流程图如图 10 所示。准备是否进入弯道 ?是否正在左转 ?否是智能车左偏是否角度控制 1 角度控制 2角度控制 3图 10 其它路径角度控制流程图程序段如下：if(analog0_ecl_or_celse upspeed=cspeed;kp=kpc;conner_analog=(AD4=AD5)?AD5:AD4;vi_ref=(max_pulse-min_pulse)*100/l_or_c)*conner_analog)/100+min_pulse; e=vi_ref-pulse;ec=pulse-b_pulse;add=kp*e/100+kd*ec/100;/速度 PD 控制s=s+add; if(supspeed) s=upspeed;if(s#include Sys_IOC7_COUNT.h#include derivative.h#include lcd.h#define PITTIME 2#define motor(speed) (PWMDTY01=(speed)#define l_or_c 100 #define left_angle_max 230#define right_angle_max 220#define center_angle 1420 #define kps_add0 20 #define kps_add1 10long kps0=0;long kps1=0; long kps2=0; long kps3=0; unsigned char kda0=100; unsigned char kda1=100; int angle=0; int jiaodu=0;unsigned char kpc=40; unsigned char kpl=35; unsigned char kd=20; unsigned char sspeed=135; unsigned char cspeed=120; int max_pulse=100; int min_pulse=50;int vi_ref=0;int s=0; int b_pulse=0;int pulse=0;unsigned int count=0,count1=0,count2=0,count3=0,count4=0,count5=0,count6=0,count7=0,count8=0,count9=0; int maxh0=0,maxh1=0,minh0=0,minh1=0; int ADvalue610=0; int AD6=0; 基于磁导航路径识别的智能车设计软件设计25int b_analog0=0,analog0=0,b_analog1=0,analog1=0;int analog0_ec=0,analog1_ec=0;unsigned char cross=0; unsigned char cross2=0; int crossspeed=180; int L1,L0; unsigned char start=0; unsigned char stop=0;unsigned char allowstop=0;int ad0,ad1,ad2,ad3;void setup() PUCR_PUPBE=1; DDRB=0X09;PORTB=0XFF;DDRA=0XFF;PORTA=0XFF; REFDV=1; SYNR=3;while(!(CRGFLG CLKSEL=0x80; PWMCTL_CON01=1; PWME_PWME1= 0; PWMPRCLK=0x44; PWMSCLA=1; PWMCAE_CAE1=0; PWMPOL_PPOL1=1; PWMCLK_PCLK1 = 0; PWMPER01 =250; PWMDTY01=0; PWMCTL_CON23=1; PWME_PWME3= 0; PWMSCLB=1; PWMCAE_CAE3=0; PWMPOL_PPOL3=1; PWMCLK_PCLK3 = 1; PWMPER23=20000; PWMDTY23=center_angle; PWME_PWME1 = 1; PWME_PWME3 = 1; 基于磁导航路径识别的智能车设计软件设计26ATD0CTL1=0x20;ATD0CTL2=0x40; ATD0CTL3=0x60; ATD0CTL4=0x03; ATD0CTL5=0x30; ATD0DIEN=0x00; PITCFLMT_PITE=0; PITCE_PCE0=1;PITMTLD0=160-1;PITLD0=PITTIME-1;PITINTE_PINTE0=1;PITCFLMT_PITE=1; PACTL = (1=max) max=ADvalueij;a0=j;if(ADvalueij=max) max=ADvalueij;a1=j;if(ADvalueij150) AD2=AD0;if(AD2-AD0150) AD0=AD2; if(AD1-AD3150) AD3=AD1;if(AD3-AD1150) AD1=AD3; void pidcontrol()int e,ec,add,upspeed,kp,conner_analog;if (PORTB_PB4=1cspeed=220;max_pulse=210;min_pulse=200; else if(PORTB_PB4=0cspe基于磁导航路径识别的智能车设计软件设计28ed=170;max_pulse=170;min_pulse=160;else if(PORTB_PB4=1cspeed=180;max_pulse=180;min_pulse=170;else if(PORTB_PB4=1cspeed=200;max_pulse=190;min_pulse=180;else if(PORTB_PB4=1cspeed=200;max_pulse=200;min_pulse=190;if(AD4l_or_celse upspeed=cspeed;kp=kpc;conner_analog=(AD4=AD5)?AD5:AD4;vi_ref=(max_pulse-min_pulse)*100/l_or_c)*conner_analog)/100+min_pulse; e=vi_ref-pulse;ec=pulse-b_pulse;add=kp*e/100+kd*ec/100;s=s+add; if(supspeed) s=upspeed;if(sAD2) jiaodu=0;if(analog0_ecright_angle_max) angle=-right_angle_max; else if(jiaoduAD3) jiaodu=0;if(analog1_ecleft_angle_max) jiaodu=left_angle_max; if(jiaodu=250)&(AD3-