freescale智能车舵机PID运算
智能车PID的算法推荐
PID( Proportional Integral Derivative )控制是最早发展起来的控制策略之一，由于其算法简单、鲁棒性(稳定性）好和可靠性高，被广泛应用于工业过程控制，尤其适用于可建立精确数学模型的确定性控制系统。
模拟PID控制系统原理框图PID是一种线性控制器，它根据给定值r in(k)与实际输出值y out(k)的差值e（t）构成控制方案，利用运放实现模拟PID的一个例子：式中：以上为典型模拟PID应用例子。
关于各调节器的作用说明：♥比例调节器P1、起调节作用与输出量和给定量的差成正比，有差就有调节作用，所以他的调节结果总是有差存在，这种调节不可消除差，所以叫这种调节为有静差调节；2、但这种调节作用快，能很快减小误差，是最常用的一种调节器！3、积分I调节慢，所以PI是最常用的一种搭配！♥积分调节器I1、是给定量与输出量的差对时间的积分，在电路里就是用给定量与输出量的差给电容充电，只要时间足够长，电容器的电压总会到达给定量，使输出量与给定量的差为零；2、积分调节器是一种无静差调节器，意思是说可调节到给定值，做到精确、准确输出；♥微分控制调节器D1、，这种控制总是以输出量与给定量的差的变化率成正比，差变化越剧烈，调节作用越大，差变化越平稳，调节作用越弱；2、这种微分调节作用，使得输出量平稳而很少波动；3、这种微分调节作用，对输出量的变化、波动产生强烈的阻尼、抑制的作用，就像摩擦力的作用；数字式PID应用：增量式PID的算式为：如果换换成智能车里的方向控制就变为：Pwm_offset=PWMMiddle+PID_P*(error-last_error)+PID_I*(error)+PID_D*(error+pre_error-2*la st_error);其中：error=middle-offset，注意这个公式里的offset应该有正负之分，左右偏移的值互为相反。
这样自然就确定了最后的方向。
智能汽车控制策略及PID算法分析
智能汽车控制策略及其PID 算法分析作者：王保磊机电1103-11221077目录背景简介一．智能汽车竞赛简介： (1)系统建立二．控制策略： (1)2.1：理论分析 (2)2.2 ： PID 控制规律的离散化 (3)2.3 ： matlab 分析 (4)2.4 ：系统校正 (9)总结三．总结： (14)一．智能汽车竞赛简介。
全国大学生“飞思卡尔”杯智能汽车竞赛是在规定的模型汽车平台上，使用飞思卡尔半导体公司的 8 位、 16 位微控制器作为核心控制模块，通过增加道路传感器、电机驱动电路以及编写相应软件，制作一个能够自主识别道路的模型汽车，按照规定路线行进，以完成时间最短者为优胜。
因而该竞赛是涵盖了控制、模式识别、传感技术、电子、电气、计算机、机械等多个学科的比赛。
摄像头组比赛规则：采用模拟或数字摄像头对赛道信息进行采集，通过硬件二值化并进行软件信息处理，获得赛道信息，采用规定的舵机和电机控制小车的行走。
相同的赛道，竞速，完成赛道时间短的队伍获胜。
下图为赛道示意图和实际赛道照片。
为保证小车一直沿着黑色引导线快速行驶，系统主要的控制对象是小车的转向和车速。
即应使小车在直道上以最快的速度行驶。
在进入弯道的时刻尽快减速，且角度的转向要适合弯道的曲率，确保小车平滑地转弯，并在弯道中保持恒速。
从弯道进入直道时，小车的舵机要转向至中间，速度应该立即得到提升，直至以最大的速度行进。
为实现上述控制思想，我们采用不同的控制方法来控制小车的转角和速度。
下图为小车的实物模型：二．控制策略。
2.1：理论分析：Ov7620 数字摄像头采集到的赛道信息为采集点灰度值，这些灰度值与设定的阀值进行比较转化为二进制信息，利用这些二进制信息可以确定赛道黑线位置，进而确定小车当前位置及理想通过曲线。
求得理想通过曲线上各点的斜率进而确定赛道类型，通过算法控制电机转速，以保证小车安全通过赛道防止侧翻和打滑。
计算小车的当前位置横坐标值即摄像头视野中线与理想通过曲线与横坐标交点的差值。
飞思卡尔智能车技术报告
第六届“飞思卡尔”杯全国大学生智能汽车邀请赛技术报告学校：队伍名称：参赛队员：带队教师：关于技术报告和研究论文使用授权的说明本人完全了解第六届“飞思卡尔”杯全国大学生智能汽车邀请赛关保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人，比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。
参赛队员签名：带队教师签名：日期：摘要随着现代科技的飞速发展，人们对智能化的要求已越来越高，而智能化在汽车相关产业上的应用最典型的例子就是汽车电子行业，汽车的电子化程度则被看作是衡量现代汽车水平的重要标志。
同时，汽车生产商推出越来越智能的汽车，来满足各种各样的市场需求。
本文以第六届全国大学生智能车竞赛为背景，主要介绍了智能车控制系统的机械及硬软件结构和开发流程。
机械硬件方面，采用组委会规定的标准 A 车模，以飞思卡尔半导体公司生产的80管脚16 位单片机MC9S12XS128MAA 为控制核心，其他功能模块进行辅助，包括：摄像头数据采集模块、电源管理模块、电机驱动模块、测速模块以及无线调试模块等，来完成智能车的硬件设计。
软件方面，我们在CodeWarrior IDE 开发环境中进行系统编程，使用增量式PD 算法控制舵机，使用位置式PID 算法控制电机，从而达到控制小车自主行驶的目的。
另外文章对滤波去噪算法，黑线提取算法，起止线识别等也进行了介绍。
关键字：智能车摄像头图像处理简单算法闭环控制无线调试第一章引言飞思卡尔公司作为全球最大的汽车电子半导体供应商，一直致力于为汽车电子系统提供全范围应用的单片机、模拟器件和传感器等器件产品和解决方案。
飞思卡尔公司在汽车电子的半导体器件市场拥有领先的地位并不断赢得客户的认可和信任。
其中在8 位、16 位及32 位汽车微控制器的市场占有率居于全球第一。
飞思卡尔公司生产的S12 是一个非常成功的芯片系列，在全球以及中国范围内被广泛应用于各种汽车电子应用中。
PID算法在智能车方向控制中的应用
PID算法在智能车方向控制中的应用智能车是指能够感知、分析、决策和执行动作的汽车。
它可以通过集成传感器、图像识别、机器学习和控制算法来实现自主导航和安全驾驶。
在智能车的控制系统中，方向控制是一个重要的组成部分。
PID算法是智能车方向控制中常用的一种算法，它能够通过实时调整车辆转向角度来实现车辆的精确控制。
PID算法是一种反馈控制算法，它通过不断地对系统输出与期望输出的差异进行检测和调整，以实现控制系统的稳定。
PID算法由比例控制、积分控制和微分控制三个部分组成。
比例控制部分根据系统输出与期望输出的差异进行比例调整，积分控制部分根据系统输出与期望输出的累积差异进行积分调整，微分控制部分根据系统输出与期望输出的变化速率进行微分调整。
这三个部分共同作用，通过实时调整控制信号来达到期望输出。
在智能车的方向控制中，PID算法可以通过以下步骤应用：1.传感器数据获取：智能车通常配备了多种传感器，如惯性传感器、陀螺仪和激光雷达等，用于感知车辆的运动状态和周围环境。
PID算法需要读取传感器数据作为反馈信号。
2.设置期望输出：根据预定的路线或目标，可以设置一个期望的转向角度作为系统的期望输出。
3.计算误差：将传感器数据中获取的实际转向角度与期望转向角度进行比较，计算得出误差。
误差可以表示为实际转向角度减去期望转向角度的差异。
4.比例控制：根据误差的大小，比例控制部分会调整控制信号的大小，从而影响车辆的转向角度。
比例系数越大，车辆的响应速度越快，但可能会引起过冲或震荡现象。
5.积分控制：积分控制部分会通过累积误差来调整控制信号，从而消除系统的稳态误差。
积分系数越大，车辆的稳定性越好，但可能会引起过冲现象。
6.微分控制：微分控制部分会通过测量误差的变化率来调整控制信号，从而降低车辆的震荡现象。
微分系数越大，车辆的稳定性越好，但可能会引起过度补偿。
7.发送控制信号：根据比例控制、积分控制和微分控制的结果，生成控制信号并发送给车辆的转向系统，从而实现转向角度的调整。
智能车PID算法实现原理讲解
智能车PID算法实现原理讲解引言智能车是一种能够通过传感器收集环境信息，然后分析、决策、控制并实现自主导航的车辆。
PID（Proportional-Integral-Differential）控制算法是智能车中常用的一种控制方法，可以实现对车辆的位置、速度等参数进行精确调节。
本文将对PID算法的实现原理进行详细讲解。
一、PID控制算法原理PID控制算法是一种经典的反馈控制算法，通过对系统的误差进行连续检测和调整，使得系统能够自动调整到期望值附近并保持稳定。
PID算法由三个部分组成：比例控制（P），积分控制（I）和微分控制（D）。
1.比例控制（P）比例控制是基于误差的当前值来进行控制调整的，它通过将误差乘以一个比例系数来调整控制量的变化。
这意味着当误差增大时，控制量的变化也会增大。
2.积分控制（I）积分控制是基于误差的历史累积值来进行控制调整的，它通过将误差的累积值乘以一个积分系数来调整控制量的变化。
这意味着当误差持续存在时，控制量的变化会逐渐增大，从而更好地调整系统。
3.微分控制（D）微分控制是基于误差的变化率来进行控制调整的，它通过将误差的变化率乘以一个微分系数来调整控制量的变化。
这意味着当误差快速变化时，控制量的变化也会被加大。
二、PID算法实现步骤基于PID算法的智能车控制实现需要按照以下步骤进行：1.设定目标值（设定值）和实际值（反馈值）。
目标值为期望的位置或速度，实际值通过传感器获取。
2.计算误差。
误差可以通过将设定值减去实际值得到。
3.计算比例控制量。
比例控制量可以通过将误差乘以比例系数得到。
4.计算积分控制量。
积分控制量可以通过将误差的累积值乘以积分系数得到。
5.计算微分控制量。
微分控制量可以通过将误差的变化率乘以微分系数得到。
6.将比例控制量、积分控制量和微分控制量相加得到最终的控制量。
7.根据最终的控制量来调整车辆的状态，例如调整轮速、转向角度等。
8.循环执行上述步骤，使得车辆能够持续地调整控制量，使得实际值逐渐接近设定值，从而实现稳定的控制。
智能车速度控制pid(电机闭环控制算法)
智能车速度控制pid（电机闭环控制算法）对于智能车的电机闭环控制算法，我之所以标题没有写上智能车电机PID闭环控制算法是因为PID 算法根本就不是特别好的适用于智能车这种变化很快的系统，对于智能车，电机的调速可以说是时时刻刻再进行调速控制的，我上面说描述的经典PID 算法，都是针对一些惰性系统，也就是说是变化比较慢的系统的，所以对于智能车的电机调速采用完完整整的PID 算法，是根本不可取的，及时采用了，你必须要经过一些变换和改进才能使用。
以上的简述只是鄙人自己的看法，如有错误，请各位高手指正。
现在估计您会疑问，PID 不适用于智能车的电机控制，那什么才适用呢？鄙人原来做过智能车，从鄙人本身的理解，P 算法控制电机，也就是比例控制是最好的，反应速度快，控制精度高，不存在积分和微分效应，非常适用于适用于控制周期短的系统，当然，对于一些特殊的逻辑控制算法，可能要采用PD算法，用微分来做补偿，防止震荡和超调。
下面来说下电机控制算法从开始的加入到最终的确定的方法：当然这一切的前提就是安装了编码器，车速有反馈，只有加上编码器，有了反馈，才能组成一个闭环系统。
当然您也可以加上码盘，或者霍尔开关等一切可以返回车速的东西都可以。
（1）首先建议在车速比较慢的时候，采用PID 算法来控制电机，为什么开始要建议您采用PID 呢？主要是为了让您更加深刻理解PID 算法的精髓和调试步骤方法等，有助于以后对控制算法更加深入的研究和书写。
调试PID 三个参数的方法，很多地方都提供了，我在这里简单的说下：首先将ID 参数都变为0，先调整P 比例参数，调整到速度基本上跟您给定的速度差不多，也就是说基本稳定在您给定的脉冲数，当然这个时候会非常的震荡，不要担心，接下来调整I，调整I 的结果就是震荡会消除很多，但是车速会变化缓慢，也就是说会有一些延迟，然后再调整D，调整D 的结果就是增强调节的灵活性和预见性，在给定速度变化的过程中，能够以一个平稳过渡来变换，而且速度可以长时间稳定在给定速度附近，然后PID 三个参数的基本范围就确定了，然后再根据实际的跑车来微调这些参数，当然在您调试PID之前，请仔细阅读PID 理论知识，这样有助于您的调试和理解，。
飞思卡尔智能车电磁组分区算法介绍
飞思卡尔智能车电磁组分区算法介绍写在之前的话:1、⽬前我是⼀名在校学⽣，这也是我第⼀次写博客，不周之处，请多谅解；2、此算法并⾮原创，借鉴⾃⼭东德州学院第⼋届⽩杨队（PS：个⼈看法，对于⼀些⼈把别⼈的开源东西改头换⾯⼀下就说是⾃⼰的原创⾏为⼗分鄙视）；3、对于此算法的理解和说明并⾮纸上谈兵，算法已经被我运⽤到了⼩车⽐赛中并取得好的成绩（具体就不多说了，⽐赛时车莫名其妙坏了，⽐赛前调试的速度绝对能进国赛，⽐较遗憾），总之这算法是我尝试过的最好的算法；4、这⼀次所介绍的只是路径算法和⼀些知识普及，后⾯有时间会介绍其余部分算法及许多好的思路（舵机电机控制思路（不只是简单的PID），双车策略）；5、希望对于这⽅⾯有涉及的⼈能与我联系并交流或指出不⾜之处。
---------------------------------------------------------------分割线-----------------------------------------------------------------------------⼀、没有这⽅⾯了解的可以看看 飞思卡尔智能车分为三组：摄像头、光电、电磁，我做的是电磁车，三种车队区别在于传感器的不同，所以获得路径信息的⽅法也不⼀样，摄像头和光电识别的是赛道上的⿊线（⽩底赛道），⽽电磁车则是检测埋在赛道下的通⼊100mh电流的漆包线，摄像头和光电采⽤的是摄像头和ccd作为传感器，电磁则是⽤电感放在漆包线周围，则电感上就会产⽣感应电动势，且感应电动势的⼤⼩于通过线圈回路的磁通量成正⽐，⼜因为漆包线周围的磁感应强度不同，因此不同位置的电感的感应电动势就不同，因此就可以去确定电感位置；因此在车⼦前⾯设置了50cm的前瞻，电感布局如下(怎么发不了图⽚)：分为两排，前排3个，编号0，1，2（前期还加了两个竖直电感⽤来帮助过直⾓弯，后来改为了⼋字电感）；后排2个，编号3，4；现在车⼦获得了不同位置的感应电动势的⼤⼩了，但这些值是不能处理的:1、感应电动势太微弱；2、是模拟信号，信号太微弱就放⼤它；这就涉及到模拟电路的知识了，就不多说了（因为要把这讲完到PCB绘制的篇幅就⾜够写另开⼀号专门写这些⽅⾯来（PS：题外话（我的题外话⽐较多））：放⼤部分外围你设计的再好也抵不过⼀个更好的芯⽚，有两个例⼦，⼀个是我⾃⼰的:之前⽤的是NE5532，但是效果不理想，加了好多什么滤波，补偿，都⽤上，没⽤，软件⾥处理后⾯再说，后来⼀狠⼼换了AD620，感觉像是春天来了，因为它是仪⽤放⼤器，还有就是贵。
飞思卡尔智能车简介
智能车制作F R E E S C A L E学院：信息工程学院班级：电气工程及其自动化132 学号：6101113078姓名：李瑞欣目录：1. 整体概述2．单片机介绍3．C语言4．智能车队的三个组5．我对这门课的建议一、整体概述智能车的制作过程包括理论设计、实际制作、整车调试、现场比赛等环节，要求学生组成团队，协同工作。
内容涵盖自动控制、模式识别、传感技术、电子、电气、计算机、机械与汽车等多学科多专业。
下面是一个智能车的模块分布：总的来说智能车有六大模块：信号输入模块、控制输出模块、数据处理模块、信息显示模块、信息发送模块、异常处理模块。
1、信号输入模块：智能车通过传感器获知赛道上的路况信息（直道，弯道，山坡，障碍物等），同时也通过传感器获取智能车自身的信息（车速，电磁电量等）。
这些数据构成了智能车软件系统（大脑）的信息来源，软件系统依靠这些数据，改变智能车的运行状态，保证其在最短的时间内按照规定跑完整个赛道。
2、控制输出模块：智能车在赛道上依靠转向机构（舵机）和动力机构（电机）来控制运行状态，这也是智能车最主要的模块，这个模块的好坏直接决定了你的比赛成绩。
电机和舵机都是通过PWM控制的，因此我们的软件系统需要根据已有的信息进行分析计算得到一个合适的输出数据（占空比）来控制电机和舵机。
3数据处理模块：主要是对电感、编码器、干簧管的数据处理。
信号输入模块得到的数据非常原始，有杂波。
基本上是不能直接用来计算的。
因此需要有信号处理模块对采集的数据进行处理，得到可用的数据。
4信息显示模块：智能车调试过程中，用显示器来显示智能车的部分信息，判断智能车是否正常运行。
正式比赛过程中可关闭。
主流的显示器有:Nokia 5110 ,OLED模块等，需要进行驱动移植。
5信息发送模块智能车的调试过程中，我们需要观察智能车的实时状态（采集的信号是否正常，输出是否正常），这个时候就需要用到信息发送模块，将智能车运行时的数据发送到电脑上就行分析处理。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
/**********************舵机增量式PID算法*********************** double ref = 0;//设置参数设定值double feb = 0;//采样反馈过程值int pwm_var = 0; //PID调整量int PWM_out = 0; //PWM输出量double Uo = 0;double Ek = 0;double Ei = 0;double Ed = 0;#define Kp 8 //PID调节的比例常数#define Ti 0.05 //PID调节的积分常数#define Td 0.02 //PID调节的微分时间常数#define T 0.02 //采样周期#define Kpp Kp * ( 1 + (T / Ti) + (Td / T) )#define Ki (-Kp) * ( 1 + (2 * Td / T ) )#define Kd Kp * Td / T//#define Kpp 4//#define Ki 0.8//#define Kd 20//误差的阀值，小于这个数值的时候，不做PID调整，避免误差较小时频繁调节引起震荡#define Emin 3//调整值限幅，防止积分饱和#define Umax 100#define Umin -100//输出值限幅#define Pmax 15500#define Pmin 200///////////////////////////////////////////////////////////////////////// PID运算 ///////void pid_ctrl(void){Ek = ref - feb; //差值运算if( fabs(Ek) < Emin ) //误差的阀值(死区控制??){pwm_var = 0;}else{Uo = Kpp*Ek + Ki*Ei + Kd*Ed;//PID计算Ed = Ei;Ei = Ek;pwm_var = (int)Uo; //制转化调整量,PWM为整数if(pwm_var >= Umax)pwm_var = Umax; //调整值限幅，防止积分饱和 if(pwm_var <= Umin)pwm_var = Umin; //调整值限幅，防止积分饱和 }PWM_out += pwm_var; //调整PWM输出if(PWM_out > Pmax)PWM_out = Pmax; //输出值限幅if(PWM_out < Pmin)PWM_out = Pmin; //输出值限幅TBCCR1 = PWM_out;//输出给寄存器,改变PWM占空比}#define G_Kp 8 //P 8#define G_Ki 15 //I#define G_Kd 0 //Dint error;int pre_error;int last_error;int U_error;uint U_Pre1=9000; //摆头舵机中心位置uint U_Pre2=8600; //方向舵机中心位置uchar sum;uchar temp,tt,ss;/**************************************************函数：delay_ms()描述：延迟x*10微秒**************************************************/void delay(uint time){while(time--){_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop); _asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);}}void display(){sum = 0;error = 0;/*********************************/PORTA = 0x00;delay(100);if(PORTA_PA0 == 1){sum++;error -= 12;}/*********************************/PORTA = 0x02;delay(100);if(PORTA_PA0 == 1){sum++;error -= 10;}/*********************************/PORTA = 0x04;delay(100);if(PORTA_PA0 == 1){sum++;error -= 8;}/*********************************/ PORTA = 0x08;delay(100);if(PORTA_PA0 == 1){sum++;error -= 6;}/*********************************/ PORTA = 0x0a;delay(100);if(PORTA_PA0 == 1){sum++;error -= 4;}/*********************************/ PORTA = 0x0c;delay(100);if(PORTA_PA0 == 1){sum++;error -= 2;}/*********************************/ PORTA = 0x10;delay(100);if(PORTA_PA0 == 1){sum++;error += 2;}/*********************************/ PORTA = 0x12;delay(100);if(PORTA_PA0 == 1){sum++;error += 4;}/*********************************/PORTA = 0x14;delay(100);if(PORTA_PA0 == 1){sum++;error += 6;}/*********************************/ PORTA = 0x18;delay(100);if(PORTA_PA0 == 1){sum++;error += 8;}/*********************************/ PORTA = 0x1a;delay(100);if(PORTA_PA0 == 1){sum++;error += 10;}/*********************************/ PORTA = 0x1c;delay(100);if(PORTA_PA0 == 1){sum++;error += 12;}PORTA = 0x17; //激光消影/*********************************/ if((sum>0)&&(sum<5)){error = (int)(error/(sum));if(error>4)U_error = 220;else if(error<-4)U_error = -220;else if(error>6)U_error = 320;else if(error<-6)else if(error>8)U_error = 420;else if(error<-8)U_error = -420;elseU_error = ((G_Kp*(error-pre_error)+G_Ki*error+G_Kd*(error-2*pre_err or+last_error)));if(U_error >420)U_error = 420;else if(U_error <-420)U_error = -420;U_Pre1 += U_error;if(U_Pre1>12000)U_Pre1 = 12000;else if(U_Pre1<6000)U_Pre1 = 6000;PWMDTY01 = U_Pre1;if(U_Pre1 > 9000) U_Pre2 = (8600+(((U_Pre1-9000)*9)/10));else U_Pre2 = (8600-(((9000-U_Pre1)*7)/10));if(U_Pre2 > 9800) U_Pre2 = 9800;else if(U_Pre2 < 7400) U_Pre2 = 7400;PWMDTY45 = U_Pre2;}last_error=pre_error;pre_error=error;/**********************///速度调节if((U_Pre2>9600)||(U_Pre2<7600)){if(temp == 6){PTP_PTP7 = 1; //电机正转delay(100);}if(temp == 1){tt++;if(tt == 1)PWMDTY3 = 50;}else{tt = 0;temp = 1;}}else if((U_Pre2>9400)||(U_Pre2<7800)){if(temp == 2){tt++;if(tt == 2)PWMDTY3 = 70;}else{tt = 0;temp = 2;}}else if((U_Pre2>9200)||(U_Pre2<8000)){if(temp == 3){tt++;if(tt == 2)PWMDTY3 = 75;}else{tt = 0;temp = 3;}}else if((U_Pre2>9000)||(U_Pre2<8200)){if(temp == 4){tt++;if(tt == 2)PWMDTY3 = 80;}else{tt = 0;temp = 4;}}else if((U_Pre2>8700)||(U_Pre2<8500)){if(temp == 5){tt++;if(tt == 2)PWMDTY3 = 83;}else{tt = 0;temp = 5; }}else if((U_Pre2<8700)||(U_Pre2>8500)) //直道{if(temp == 6){tt++;if(tt == 2)PWMDTY3 = 85;}else{tt = 0;temp = 6; }}/************************************************************/ }。