哈尔滨工程大学-极品飞车3号

哈尔滨大学-极品飞车3号 第八届“飞思卡尔”杯全国大学生 智能汽车竞赛 基于电磁传感器的智能车路径识别 控制系统 学 校:哈尔滨工程大学 队伍名称:极品飞车三号 参赛队员:刘鹏 何超 陈美远 带队教师:张爱筠 管凤旭 关于技术报告和研究使用授权的说明 本人完全了解第八届“飞思卡尔”杯全国大学生智能汽车邀请赛关保留、使用技术报告和研究论文的规定，即:参赛作品著作权归参赛者本人，比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。 参赛队员签名: 带队教师签名: 日 期: 摘 要 本文对哈尔滨工程大学极品飞车3号进行了详细的介绍。本智能车k60为主控器,通10mH电感检测出导线产生的磁场引导小车在规定路径上行驶,小车能够实现自主识别赛道,能够稳定行驶、保持较高的速度。在六个多月的准备过程中,我们对智能车的机械结构、硬件、软件都分别进行了深入的学习研究。机械方面,参照汽车的行驶原理,从材料、结构上进行改动对系统的硬件设计采用模块化的设计方法。主要包括,单片机主控模块、电源模块、电磁传感器模块、速度检测模块、舵机驱动模块,电机驱动模块。对系统软件设计则是应用IAR嵌入式系统开发软件,自行设计了智能汽车自动识别路径的方案,编写出基于K60 的路径识别程序和速度控制程序。 目 录 摘 要............................................................. 3 第一章 引 言.................................................... 5 1.1设计思路及方案的总体说明 ....................................................................................................................5 1.2章节安排 .......................................................................................................................................................5 第二章 机械设计.................................................. 6 2.1车体参数要求...............................................................................................................................................6 2.2车体参数调整...............................................................................................................................................6 2.2.1主销后倾 ..............................................................................................................................................6 2.2.2 前轮前束 .............................................................................................................................................7 2.2.3 车体重心 .............................................................................................................................................7 2.3传感器支架与传感器安装 ........................................................................................................................8 2.4舵机安装 .......................................................................................................................................................8 2.5车胎的处理 ...................................................................................................................................................9 2.6差速调节 .......................................................................................................................................................9 第三章 硬件电路设计.............................................. 10 3.1 电磁传感器的设计 ...................................................................................................................................10 3.2 PCB主板的设计.........................................................................................................................................14 3.3 电源管理模块 ............................................................................................................................................15 3.4 K60核心控制模块 ..................................................................................................................................16 3.5 电机驱动.....................................................................................................................................................16 第四章 软件设计................................................ 17 4.1 传感器的测量算法 ...................................................................................................................................18 4.2 舵机的PD控制..........................................................................................................................................19 4.3 码盘的速度反馈 .......................................................................................................................................20 4.4速度PID控制算法及其改进形式 .........................................................................................................21 第五章 开发调试过程及主要参数.................................... 22 5.1开发工具 .....................................................................................................................................................22 5.2制作调试过程说明....................................................................................................................................23 5.3 智能车主要技术参数...............................................................................................................................23 第六章 总 结................................................... 24 6.1 存在的问题及改进方法 ..........................................................................................................................24 6.2心得与总结 .................................................................................................................................................25 参考文献.......................................................... 26 参考程序.......................................................... 26 第一章 引 言 1.1设计思路及方案的总体说明 根据电磁车循迹的要求，电磁车设计包括赛车的设计和恒流源的设计。 根据电磁传感器方案设计，赛车共包括六大模块:电磁传感器模块、速度传感器、起跑线检测模块、K60核心控制模块，电机驱动模块和电源管管理模块。 硬件四倍频光电码盘 电机驱动直流电机起跑线检测部分K60核心处理模块伺服舵机信号处理电磁传感器 电源管理 电池 图2.1 赛车系统结构框图 1.2章节安排 第一章引言;详细介绍了本智能车系统的总体概况; 第二章智能车机械调校:详细介绍了本智能车在机械结构上的调整，包括 前轮的调整，舵机的固定，等; 第三章智能车硬件系统:详细介绍了本智能车各个硬件模块电路的设计、 制作和方案的选择; 第四章智能车软件系统:主要介绍了本智能车软件设计、算法设计，详细 的介绍了舵机控制和速度控制; 第五章开发调试过程及主要参数:详细介绍了智能车软件仿真调试平台、智 能车主要技术参数等; 第六章总结:总结整个设计的过程和对未来设级的展望。 第二章 机械设计 2.1车体参数要求 1) 禁止改动车底盘结构、轮距、轮径及轮胎,如有必要可以对于车模中的零部件进行适当删减。 2) 禁止采用其它型号的驱动电机,禁止改动驱动电机的传动比, 3) 禁止改造车模运动传动结构, 4) 禁止改动舵机,但可以更改舵机输出轴上连接件, 5) 禁止改动驱动电机以及电池,车模前进动力必须来源于车模本身直流电机及电池, 6) 禁止增加车模地面支撑装置。在车模静止、动态运行过程中,只允许车模原有四个车轮对车模起到支撑作用。对于光电平衡组,车模直立行走,在比赛过程中,只允许原有车模两个后轮对车模起到支撑作用。 7) 为了安装电路、传感器等,允许在底盘上打孔或安装辅助支架等。 8)电磁组车模改装完毕后，车模尺寸宽度不超过250mm，长度没有限制。 2.2车体参数调整 2.2.1主销后倾 主销在车模的纵向平面内(汽车的侧面)有一个向后的倾角γ，即主销线 与地面垂直线在车模在纵向平面的夹角，称为“主销后倾角”。车模采用主销后 倾的原因是由于汽车在车轮偏转后会产生一回正力矩，纠正车轮偏转。我们车采用增加一个垫片来增加一个大约5度左右的后倾角。如图2.2.1 图2.2.1主销后倾 2.2.2 前轮前束 前束是转向灵敏度与稳定性的权衡。前束不可以无限度增大，太大了的话，直道行驶时车轮与地面发生的就不是滚动摩擦，而是滑动摩擦，轮胎磨损将急剧增大，且会导致阻力加大，降低直道速度。过度减小，会导致稳定性降低，车辆抖动，难以操控。为了增加车子的转向性能以及满足阿克曼转弯原理，将前束调为俗称的“外八”大概8度左右。 2.2.3 车体重心 因为车体的重心将决定车辆行驶的稳定性和转弯特性，特别是在车高速行驶时，重心的位置与高低对智能车影响的很大的。经过反复试验，我们选择将车辆的重心适当的前移，使重心处于电池与主控板中间，这样有利于增加车辆的转向性能。还有选择尽量的将车体的重心降低(保证车过路障的时候能不触碰路障)。如图2.2.3 2.3传感器支架与传感器安装 选取传感器支架,我们选用质地轻的碳杆,经过反复试验,选用较粗的碳杆转动惯量较大,转向笨重,选用较细的碳杆,强度不够,行驶时晃动较大,对信号采集影响很大,最后我们选择6*3*100mm碳圆杆作为我们的传感器支架,其重量和强度都适中,适合我们的智能小车。 传感器的安装时,电磁组别不限制长度,我们把传感器安放到合适的长度,有意的增加前瞻。我们传感器采用三个角度对赛道磁场检测,这样能更精确的检测出赛道的信息,经过计算进而选择出最优的行驶路径,更利于跑出最快速度。 如图2.3 图2.3传感器安装 2.4舵机安装 舵机安装直接影响到转向问题。如果舵机调整不到位,将很大程度上限制转 向角度和转向灵敏度。车模原始舵机被安放在了车模中心,为了空间的合理利用,我们把舵机转移到了两前轮之间,为小车主板留出了一定的空间。由于舵机原始只有一个扭转点,存在一定的虚位,我们将其改装成两个扭转点,很好的解决了舵机存在虚位的问题。我们还垫高了舵机,加长了舵机转动半径,使舵机转向更轻盈、灵巧。如图2.4.1 图2.4.1舵机安装 2.5车胎的处理 新车轮胎与KT板的摩擦不是很好，我们选择对轮胎面进行打磨，磨掉不的部分，然后让车在小赛道上长时间行驶，使得车胎与赛道接触面更大，增大其摩擦力。选择车轮内胎时，我们选择薄海绵，这样更能加大接触面，增大摩擦力。 2.6差速调节 A车模采用的是滚珠差速，这种差速没有齿轮差速好，但是对于小车来说足够。首先必须要保证差速不打滑，保证快速的加减速。差速盘过松会导致差速打滑，过紧差速性能降低。 在调整差速过程中，根据转弯的性能对差速松紧进行调整，并且在差速滚珠盘上涂上专业差速油，反复的赛道测试，使得赛车更适应差速。 如图2.6.1 图2.6.1差速度计 第三章 硬件电路设计 3.1 电磁传感器的设计 考虑到电流的大小，方向和频率，依据经可靠、稳定、经济的要求，我们选择10mH的电感，然后进行滤波、放大、检波;其电路图如下: 图3.2.1磁场信号采集电路 利用LC谐振，获得合适幅值的交流信号，在通过两级运放将信号放大，第二级增益可调，以消除不同电感品质上的微弱差别。在运放交流放大之后加入倍压检波电路，可以得到与交流的峰峰值成正比的直流信号，供单片机自带AD模块采集。 图3.2.2信号处理multisim仿真图 图3.2.3一级放大波形图 图3.2.4检波电路效果图 根据电磁传感器中电感的方向与磁场的关系，采用垂直方向放置时，其测量的磁场大小为Bx，且根据比奥萨伐尔定义可知: 其特性如下图所示: 图3.2.5 垂直方向传感器测量理论图 垂直方向传感器测量出的电压大小为: 根据实验测定的数据，利用MATLAB可得下图: 图 3.2.6 垂直方向传感器测量仿真图 可得出以下结论:实验数据与理论基本相同。 采用水平方向放置时，其测量的磁场大小为Bz，且根据比奥沙法尔定义可 知: 水平方向电磁传感器测出的电压大小为: 其特性如下图所示: 图3.2.7 水平方向传感器测量理论图 根据车与导线的位置关系如图: 其中: AE = l ;h为传感器距赛道的高度; A 根据实验测得的数据，我们采用了垂直位置的传感器，根据电磁传感器采集出的电压的大小，两相邻的传感器的距离去3,5cm左右，我们采用相邻的两传感器 。 间距为4cm 3.2 PCB主板的设计 要求主板的面积小，质量轻，与整体结构相融合，便于安装同时具有通用性和扩展性，根据实际需求设计的主板PCB如下图所示: 图3.2.5主板PCB顶层 图3.2.6主板PCB底层 3.3 电源管理模块 把电压为7.2V的电池经LM2940稳压后，其电路图如下: 图3.2.7电源部分电路 电源管理可以完成如下功能: 经过稳压芯片AMS1117-3.3稳压后，输出3.3V电压以驱动单片机工作; 经过稳压芯片LM2940稳压后，输出5V电压再转换为正负5V以对运放供电; 经过稳压芯片LM2940稳压后，输出5V电压对光码盘供电; 经过二极管降压后给伺服舵机供电。 3.4 K60核心控制模块 由于开始开发的时候使用的是龙邱的K60最小系统，接口等电路与其相同，故系统控制核心为龙邱最小系统，如下图所示: 图3.2., K60核心控制部分 3.5 电机驱动 利用MOS管构成双极性H桥,利用PWM控制，桥的导通和关断,使电流更大,驱动能力更强,再结合逻辑电路,使其使用更安全,更稳定,如下图所示, 图3. 5 电机驱动实物图 操作时序如下, 输入,，: 输入,，, 电机转向 0 1 正转 1 0 反转 0 0 停车 1 1 停车 第四章 软件设计 为了体现程序的模块性和可移植性,我们把程序分成各个模块进行分别处理,而各模块也有相应的形参做为接口,可以做到可移植性,通用性。下面就这些模块的设计与实现分别进行详细阐述。 程序中用到的硬件资源有定时器模块、PWM模块、AD模块、脉冲累加器、普通I/O口,主程序流程图如下, 4.1 传感器的测量算法 公用了八个电感作为传感器,其中六个用于判断导线位置, 当某个传感器离导线最近时能与导线保持垂直,根据采集到的数据,用MATLAB绘制出图形,并用直线拟合,采集到的电压如下、MATLAB 代码如下, A=[86 108 141 184 229 267 283 263 227 185 144 108 83]/283; B=-6:1:6; plot(B,A); grid minor; C=[108 141 184 229 267]/283; D=-5:1:-1; y1 = polyfit(D,C,1); x = -6:0.1:0; y=40.6/283*x+307.6/283; hold on; plot(x,y); 如图的直线与原曲线在2—5cm处拟合很好,故用此线性段来计算导线位置。 4.2 舵机的PD控制 根据电磁传感器采集到的偏移量，以偏移量以及偏移量的微分量作为输入量对舵机进行位置式PD控制。舵机相对于中值的转角为: ,,Kp*b，Kd*Diff 其中Kp为比例系数; b为相对于中心的偏移量; Kd为积分系数; Diff为位置微分量 输入捕捉中断处理函数 清中断标志位4.3 码盘的速度反 N馈 捕捉上升降沿, Y 增量式编码器的输 出经过一个固定周期定时器溢出中断 求捕捉寄存器与保的脉冲采集，采集到脉持寄存器的差值清中断标志位冲的个数，即代表当前计算转速定时器中断 次数标志加1速度，其程序流程图如保存转速 下: 中断返回 开输入捕捉中断 Y 中断返回 4.4速度PID控制算法及其改进形式 PID 控制是工业过程控制中历史最悠久,生命力最强的控制方式。这主要 是因为这种控制方式具有直观、实现简单和鲁棒性能好等一系列的优点。 位置式PID算式连续控制系统中的PID控制规律是 t1de(t)，，x(t),K[e(t)，e,d,，T]，x0pd,0Tdti 4.1 x0其中是偏差信号为零时的控制作用,是控制量的基准, 利用外接矩形法进行数值积分,一阶后向差分进行数值微分,当选定采样周 期为T时,式4.1可离散为下面的差分方程 kTTdx(t),K[e(t)，e，(e,e)]，x,,10pjkkTT,0ji 4.2 增量式PID算式。 根据式4.2得出 kTTdx,K[e，e，(e,e)]，x,,,,,11120kpkjkkTT,0ji 4.3 于是 TTd,x,x,x,K[e,e，e，(e,2e，e)]kkk,1pkk,1kkk,1k,2TTi 4.4 式4.3的计算结果,反映了第k和第k,1次输出之间的增量,所以称为增量算式。这个算式的结果是可正可负的。利用增量算式控制执行机构,执行机构每次只增加一个增量,因此执行机构起了一个累加的作用。 对于整个系统来说,位置和增量式两种算式并无本质区别,只是将原来全部由计算机完成的工作,分出一部分由其他元件去完成。然而,虽然增量式算式只是算法上的一点改进,却带来了不少优点,算式只与最近几次采样值有关,不需要进行累加,不易引起误差累积,因此容易获得较好的控制效果。计算机只输出增量,误动作时影响小,必要时可加逻辑保护,限制或禁止故障时的输出。手动于自动切换时,由于步进电机具有保持作用,所以容易实现无扰动切换,机器故障时,也可以把信号保持在原位。由于增量算式有上述优点,我们的直流电机采用增量式PI控制。 第五章 开发调试过程及主要参数 经过我们组队员的共同开发，设计，验证，我们依据所学的知识从入门到提高，经历的了开发工具的应用、方案的设计、调试过程如下: 5.1开发工具 我们采用了USB接口的J-Link调试工具，是支持Freescale MK60系列32位单片机的调试工具。 软件上采用的是IAR Embedded Workbench编译器,经过源程序的编写、编译、连接、通过J-Link下载至单片机，完成单片机的开发过程。IAR Embedded Workbench编译器是全球领先的嵌入式系统开发工具，它支持众多知名半导体公司的微处理器，包括集成开发环境IDE，处理器专家库，全芯片仿真，可视化参数显示工具，项目管理器，C交叉编译器，汇编器，链接器以及调试器，支持在 线编程和调试，再配合上J-Link下载器，给我们的开发，设计工作带来了很大的方便。 5.2制作调试过程说明 各硬件部分我们通过制作PCB电路板安装在智能车的相关部位。在车头我们制作了寻迹用的电磁传感器电路板，为了使传感器板质量最轻，我们把放大电路另画了一块电路板放在后边。当输入激励电源后，放大电路将放大后的信号通过排线传给单片机 。主控电路板安装在车体中间，包括了电源管理电路、单片机接口电路，终点识别部分和码盘倍频模块，留有电磁传感器信号输入接口、速度传感器信号输入接口、电源输入接口、舵机控制接口、电机接口等输入输出端口。 由于有前几届的基础，我们没有在电源上花费很多时间。 车的重心问题一直是困扰我们的地方，车的重心前后方向调整，对智能车行驶性能有很大影响。按照车辆运动学理论，车身重心前移，会增加转向，但会降低转向的灵敏度，同时降低后轮的抓地力;重心后移，会减少转向，但会增大转向灵敏度，后轮抓地力也会增加。因而调整合适的车体重心，让智能车更加适应跑道是很关键的。根据实际调试经验，鉴于当前舵机响应较迟缓，因此，需要将车的重心由中点稍向前移，增加转向性能。 电机驱动是最脆弱的地方，有时会出现短路或起火的现象，由于我们的驱动加入了逻辑控制单元，操作的失误不会引起这种想象，我们经过试验和判断分析，原因可能是由于先给了驱动PWM信号，这时如果给驱动加上电压，由于这时加上了一个阶跃信号，导致上下桥臂的MOS管导通了，由于MOS管的电阻很小，经过的电流很大而导致MOS管烧毁。 经过我们组成员将近两个月的共同努力，我们达到了:在直道上，模型车的行驶的速度可达3m/s, 直道入弯道时，根据弯道曲率半径的不同，模型车会相应的减速，同时伺服舵机转过一低的角度，平均速度达到了2.4m/s，且稳定性和快速性良好。基本实现了预期目标。 5.3 智能车主要技术参数 模型车的外形如下图所示: 主要技术参数如下: 项目 参数 车模几何尺寸(长、宽、高)(毫米) 750×250×150 电路电容总量(微法) 约1100uF 传感器种类及个数 电磁传感器×8;光电码盘×1, 干簧管×6 主要芯片 LM2940×2; K60N512VMD100×1 车模重量 1.3Kg 赛道检测精度 10mm 电机个数 舵机×1;电机×1 赛道检测频率(AD检测频率) 1KHz 功率(各部件都工作时) 15W 第六章 总 结 6.1 存在的问题及改进方法 1. 电感的绕制都是手工,每个电感都有一定的误差,并且不同批次绕制的电感误差较大,对于采集的数据有一定的影响,采集出的数据有不稳定。在选择电感时应该重同一批次中选择出特性良好的电感。 2. 车体重心过于靠前,对车体的稳定性有影响,特别是在高速过弯道的时候, 容易出现甩尾现象,可以将主板的位置再优化一下,适当靠后,把电池也可以往后移动,可以使得后轮摩擦力更大一些,或者做一个主动悬挂装置改变后轮的受力。 3. 赛道识别方面对各种赛道的识别还不是很准确,可以通过反复的测试,优化算法。 6.2心得与总结 在这半年多的比赛中,在场地、经费方面都的到了学校和学院的大力支持,在此特别感谢一直支持和关注飞思卡尔智能车比赛的学校和学院领导以及各位老师。同时也感谢比赛组委会能组织这样一项很有意义的比赛。 从2012年12月比赛规则下来,我们便展开了比赛的准备,从查找文献资料,机械设计,硬件电路设计,到组车调试,软件编程,系统调试。这一路走来,遇到了一个又一个问题,发现一个就解决一个,并总结提高。我们组的每一个人都在为我们的小车跑得更快更稳而努力着,团结就是力量,我们一直坚守着这一信念。 现在,面对即将到来的大赛,多少个日日夜夜的辛苦劳动,我们有信心在比赛中取得优异成绩。我们将这半年多的努力注入这份技术报告,凝聚着我们小组每个人的心血和智慧,它也是我们小组友谊的见证,它仿佛就是一段史书,记录着在我们飞思卡尔团队里的经历,这段经历也将刻在我们的心里。 参考文献 [1] 卓晴，黄开胜，邵贝贝 学做智能车 北京航空航天大学出版社 2007 [2] 王宜怀，吴瑾，蒋银珍 嵌入式系统原理与实践 电子工业出版社 2012 [3] 杨东轩 王嵩 ARM Cortex-M4 自学笔记_基于Kinetis K60 2013 [4] 李宁，刘启新 电机自动控制系统 机械工业出版社，2003 [5]钱江一号 第六届“飞思卡尔”杯全国大学生智能车大赛技术报告 杭州电子科技大学 2011 参考程序 附录: #include "MK60N512VMD100.h " /* include peripheral declarations */ #include "includes.h" #include #include #define GPIO_PIN_MASK 0x1Fu //0x1f=31,限制位数为0--31有效 #define GPIO_PIN(x) (((1)<<(x & GPIO_PIN_MASK))) //把当前位置1 void main(void) { //开启各个GPIO口的转换时钟 SIM_SCGC5 = SIM_SCGC5_PORTA_MASK | SIM_SCGC5_PORTB_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTD_MASK | SIM_SCGC5_PORTE_MASK; DisableInterrupts; //禁止总中断 pllinit80M(); hw_pit_init(0,6500000); enable_pit_interrupt(0); hw_pit_init(1,25000); //65000=10ms 4ms LCD_Init(); // enable_pit_interrupt(1); FTM0_PWM_init(); FTM1_PWM_init(); //初始化电机pwm，频率为:10khz FTM2_PWM_init(); //初始化舵机pwm，频率为:50hz lptmr_pulse_counter(LPTMR_ALT2); //脉冲计数 pc5 返回值 = LPTMR0_CNR; adc_init(1,12); adc_init(0,12); uart_init (UART0,38400,9600); //38400 /*干簧管*/ gpio_init (PORTC,4, 0,1); gpio_init (PORTC,8, 0,1); gpio_init (PORTC,6, 0,1); gpio_init (PORTC,7, 0,1); /*拨码开关*/ gpio_init (PORTE,9, 0,0); gpio_init (PORTE,8, 0,0); gpio_init (PORTE,11, 0,0); gpio_init (PORTE,10, 0,0); /*发光二极管*/ gpio_init (PORTA,14, 1,1); gpio_init (PORTA,15, 1,1); gpio_init (PORTA,16, 1,1); gpio_init (PORTA,17, 1,1); /*数码管*/ gpio_init (PORTE,0, 1,0); gpio_init (PORTE,1, 1,0); gpio_init (PORTE,2, 1,0); gpio_init (PORTE,3, 1,0); gpio_init (PORTE,4, 1,0); gpio_init (PORTE,5, 1,0); gpio_init (PORTE,6, 1,0); gpio_init (PORTE,7, 1,0); /**anjian**/ gpio_init (PORTA,25, 0,0); gpio_init (PORTA,27, 0,0); EnableInterrupts; //开总中断 系统初始化结束 for(;;) //根据拨码开关选择速度方案，并初始化各个电感最大值 { // sprintf(str, "%6d", id); // uart_sendN(UART0,str,9); one=LPLD_GPIO_Get_b(PORTE,9); //向右为1 two=LPLD_GPIO_Get_b(PORTE,8); three=LPLD_GPIO_Get_b(PORTE,11); four=LPLD_GPIO_Get_b(PORTE,10); two=two<<1; three=three<<2; // four=four<<3; five=one|two|three; led(table[five]); if(five==0) { speedmax=98; speedmin=70; } else if(five==1) //较稳，15.6 { speedmax=95; speedmin=69; } else if(five==2) { speedmax=92; speedmin=70; } else if(five==3) { speedmax=90; speedmin=70; } if(five==4) //较稳，15.6 { speedmax=80; speedmin=60; } else if(five==5) { speedmax=80; speedmin=80; } else if(five==6) { speedmax=70; speedmin=70; } else if(five==7) { speedmax=55; speedmin=55; } if(LPLD_GPIO_Get_b(PORTA,27)==1) { delay(15000); if(LPLD_GPIO_Get_b(PORTA,27)==1) { while(LPLD_GPIO_Get_b(PORTA,27)); gpio_ctrl(PORTA,14,0); for(;;) { ADval[3]= ad_once(1,10) ; //b10 ADval[2]= ad_once(1,11) ; //B3 ADval[1]= ad_once(1,12) ; //c0 b11 ADval[0]= ad_once(1,13) ; //c0 ADval[4]= ad_once(0,15) ; ADval[5]= ad_once(0,13) ; ADval[6]= ad_once(0,12) ; //zuo ADval[7]= ad_once(0,14) ; if(ADval[4]>ADvalmax[4]) ADvalmax[4]=ADval[4]; if(ADval[5]>ADvalmax[5]) ADvalmax[5]=ADval[5]; SensorInit(); if(LPLD_GPIO_Get_b(PORTA,25)==1) { delay(15000); if(LPLD_GPIO_Get_b(PORTA,25)==1) while(LPLD_GPIO_Get_b(PORTA,25)==1); gpio_ctrl(PORTA,14,1); break; } } break; } } } //第二次初始化，初始化临界电感值 for(;;) { ADval[3]= ad_once(1,10) ; //b10 ADval[2]= ad_once(1,11) ; //B3 ADval[1]= ad_once(1,12) ; //c0 b11 ADval[0]= ad_once(1,13) ; //c0 ADvalproption(); //AD转换归一化处理 if(Proption[0]>p0max) { p1min0=Proption[1]; p0max=Proption[0]; } if(Proption[3]>p3max) { p2min3=Proption[2]; p3max=Proption[3]; } if(Proption[1]>p1max) { p2min1=Proption[2]; p0min1=Proption[0]; p1max=Proption[1]; } if(Proption[2]>p2max) { p1min2=Proption[1]; p3min2=Proption[3]; p2max=Proption[2]; } if(Pe==0) gpio_ctrl(PORTA,14,0); else gpio_ctrl(PORTA,14,1); if(Pe==1) gpio_ctrl(PORTA,15,0); else gpio_ctrl(PORTA,15,1); if(Pe==2) gpio_ctrl(PORTA,16,0); else gpio_ctrl(PORTA,16,1); if(Pe==3) gpio_ctrl(PORTA,17,0); else gpio_ctrl(PORTA,17,1); DevCal(); //找到当前位置偏差 if(LPLD_GPIO_Get_b(PORTA,27)==1) { delay(20000); if(LPLD_GPIO_Get_b(PORTA,27)==1) while(LPLD_GPIO_Get_b(PORTA,27)==1); break; } } adc_convertstart(1, 10); enable_adc_irq(); lptmr_pulse_counter(LPTMR_ALT2); delayms(55530); enable_pit_interrupt(1); //至此所有初始化结束 for(;;) { init=0; //初始化完成 if(adready==1) { ADval[4]= ad_once(0,15) ; ADval[5]= ad_once(0,13) ; //左电感 ADval[6]= ad_once(0,12) ; ADval[7]= ad_once(0,14) ; if(ADval[5]- ADval[4]>500||ADval[4]- ADval[5]>500) { tubian=1; } adc_convertstart(1, 10); adready=0; enable_adc_irq(); ADvalproption(); //AD转换归一化处理 if(ADval[5]>400&&ADval[4]>400||Proption[0]+Proption[1]+Proption[2]>240||Proption[3]+Propt ion[2]+Proption[1]>240||Proption[0]+Proption[3]>140||(Proption[3]>Proption[2]&&Proption[0]> Proption[1])) shizi=1; else shizi=0; if(Proption[2]+Proption[1]>200&&podaoover==0) { podaojs++; if(podao==0) { if(podaojs>6&&Proption[0]+Proption[3]<65) podao=1; podao1=0; } else if(podao1==1) { podao=0; podao1=0; podaoover=1; } } else { podaojs=0; } if(podao==1&&Proption[2]+Proption[1]<120) podao1=1; { if(Proption[1]>100) Proption[1]=100; if(Proption[2]>100) Proption[2]=100; } DevCal(); //找到当前位置偏差 id=Pe1-ie[1]; if(id>5) { sprintf(str, "%6d", id); uart_sendN(UART0,str,9); } Kp_p=Kp[0][(int)abs(Pe1/100)]; if(abs(Pe1)>2500) Kp_p=8.2; if(Pe1*id>0) Kd_p=0.14; else Kd_p=0.15; if(shizi==0) { if(abs(Pe1)<1500) { Pe3=-(ADval[5]- ADval[4])/3; if(Pe1<0) Pe3-=ADvalmax[5]/3*Pe1/1400; if(Pe1>0) Pe3-=ADvalmax[4]/3*Pe1/1400; } else { Pe3=-(ADval[5]- ADval[4])/4; } if(Pe3>500) Pe3=500; if(Pe3<-500) Pe3=-500; } else Pe3=0; if(abs(Pe3)<60) kp3=0; else if(abs(Pe3)<100) kp3=0.15; else if(abs(Pe3)<150) kp3=0.2; else if(abs(Pe3)<200) kp3=0.25; else if(abs(Pe3)<250) { kp3=0.3; } else { kp3=0.35; } if(abs(Pe3)<60) kpe3=0; else if(abs(Pe3)<100) kpe3=0.08; else if(abs(Pe3)<150) kpe3=0.1; else if(abs(Pe3)<200) kpe3=0.12; else if(abs(Pe3)<250) kpe3=0.13; else kpe3=0.15; Kp_p+=kp3; Pos = (Kp_p/100)*Pe1*0.96+kpe3*Pe3+Kd_p*id; idbef=id; if(Pos>210) Pos =210; //zuo限幅 if(Pos <-200) Pos =-200; //you限幅 FTM2_C0V = dmid-Pos; //舵机 if(Pos>150) qipao=1; if(LPLD_GPIO_Get_b(PORTC,4)+LPLD_GPIO_Get_b(PORTC,7)+LPLD_GPIO_Get_b(PORT C,6)+LPLD_GPIO_Get_b(PORTC,8)<3) { if(qipao==1) stop1=1; } if(stop1==1) { speed=0; if(speed1<2) { FTM1_C1V = 0; FTM1_C0V = 0; disable_pit_interrupt(1); } } else SpeedSet(); } } }