资源描述
第七届“飞思卡尔”杯全国大学生智能汽车竞赛技 术 报 告学 校：中山大学队伍名称：电磁2队参赛队员：李小锟黄志杰誉洪生带队教师：成 慧钱 宁关于技术报告和研究论文使用授权的说明本人完全了解第七届“飞思卡尔”杯全国大学生智能汽车竞赛关保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人，比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。参赛队员签名：带队教师签名：日 期：摘 要本论文中设计的智能车系统采用飞思卡尔公司的kinetisK60作为控制核心，以IRF3205和IRF4905为电机驱动模块，通过编码器获得电机速度信息。使用由电感及放大电路组成的传感器获取赛道信息；在智能车控制方面，基于位置式及增量式的PID控制算法在高速行驶赛车的路径优化和速度控制上有着不错的效果。为了提高车的速度和稳定性，我们使用VC、无线通讯模块、等多种调试工具。结果表明，本系统可靠有效。关键词：kinetisK60，IRT3205, IRF4905，PIDAbstractWe design the smart car system based on the control of kinetisK60 in this paper. We choose IRF3205 and IRF4905 to be the motor drive, and encoder to feedback the speed of the motor. We use electrical inductance to get the information of the track. During the control of the smart car, the positional PID and Increasing PID perform good on path optimization and speed control in high speeding pattern. In order to improve the speed and the stability of the smart car, we use kinds of methods, such as VC, wireless module . It turned out that the system is reliable and effective. Keyword：kinetisK60，IRT3205, IRF4905，PID目录摘要Abstract第一章 引言1第二章系统总体设计22.1 系统概述22.1.1 任务分解2直立控制分析3速度控制分析42.1.4 转弯控制分析42.2 车模倾角检测42.3整车布局42.4结构框图5第三章 系统机械结构设计与实现73.1 车模73.2车模车身简化及固定73.3陀螺仪及加速度计固定结构设计83.4 路径传感器固定结构设计93.5编码器的安装设计93.6主动轮松紧程度的调整10第四章 硬件系统的设计与实现114.1 硬件设计总体方案114.2 系统电路的设计与实现114.2.1 电源管理电路114.2.2 电机驱动电路124.2.3 计数器电路134.2.4平衡传感器电路134.2.5 路径检测电路144.3 静电处理14第五章 软件系统的设计与实现165.1 主程序165.1.1程序运行流程165.1.2PWM输出165.1.3直立控制195.1.4 速度控制195.1.5转弯控制195.2 卡尔曼滤波195.3 启动方式20第六章 开放工具与系统调试226.1 开发工具226.2 无线通信模块23第七章 车辆参数24第八章 总结24致谢I参考文献II附录A附录B第一章 引言全国大学生智能汽车比赛是经全国高等教育司研究，委托高等学校自动化专业教学指导分委会主办的，旨在培养创新精神、协作精神，提高工程实践能力的大学生科技创新活动。本次大赛中，使用竞赛秘书处统一指定的C车竞赛车模套件，采用飞思卡尔半导体公司的32位微控制器kinetisK60作为核心控制单元，自主构思控制方案进行系统设计，包括传感器信号采集处理、电机驱动以及控制算法软件开发等。制作一个能够自主识别路线的两轮自平衡智能车，让车模以两个后轮驱动在专门设计的跑道上自动识别道路行驶。本技术报告主要对小车的整体设计思路，硬件与软件的设计及实现，机械结构的安装及优化以及赛车的调试的过程作简要的说明。第二章 系统总体设计2.1系统概述系统概述图如图2.1所示 图2.1系统概述2.1.1任务分解两轮赛车通过检测自身倾斜角度以及倾斜角速度的大小，通过单片机处理采集得到的角度、角速度信号，实现对赛车自身平衡以及前进的控制，再以检测通过以20KHZ、100mA的导线的电磁场为基础，通过单片机处理采集到得磁感应电压信号，实现对赛车的转向控制，进而识别赛道达到路径讯迹的目的。车模的运行可以分为三个过程：直立、前进、转弯。但是暂时找不到其他方法可以不依赖电机而可以控制任意一个过程，所以只能由控制车模两个电机旋转方向及速度实现对车模的控制，因此将车模运动控制任务分解成以下三个基本任务： （1） 控制车模直立：通过控制两个电机正反向运动保持车模直立状态； （2） 控制车模速度：通过控制两个电机转速速度实现车模行进控制；（3） 控制车模转向：通过控制两个电机之间的转动差速实现车模转向控制。2.1.2直立控制分析如图2.1.2所示，车身可看作是一个倒立摆，要想通过控制使得倒立摆能够像单摆一样，稳定在垂直位置，只有两个办法：一个是改变重力的方向；另一个是增加额外的受力，使得恢复力与位移 方向相反才行。但显然能够做的只有第二种方式。控制倒立摆底部车轮，使得它作加速运动。这样站在小车上（非惯性系）看倒立摆，它就会受到额外的力(惯性力) ，该力与车轮的加速度方向相反，大小成正比。这样倒立摆所受到的回复力为（2-1）式中，假设控制车轮加速度与偏角成正比，比例为k1。显然，如果k1g ，（是重力加速度）那么回复力的方向便于位移方向相反了。此外，为了使得倒立摆能够尽快地在垂直位置稳定下来，还需要增加阻尼力，与偏角的速度成正比，方向相反。因此式（2-1）可变为（2-2）所以车模车轮加速度的控制算法为所以我们需要测量小车的角度和角速度即可控制直立。2.1.3速度控制分析车模电机的转速与施加在其上的电压成正比。通过传统的 PID 反馈控制，便可以精确控制电机的运行速度，从而控制车模的运行速度。不过前提是保持车身平衡。2.1.4转弯控制分析利用电磁线偏差检测信号分别与车模速度控制信号进行加和减，左右轮差动控制电压，使得车模左右轮运行角速度不一致进而控制车模方向。2.2 车模倾角检测通过加速度计检测回来的数据与重力加速度比较，可以得到车身倾角，再对此信号进行微分便可以获得倾角加速度。但在实际车模运行过程中，由于车模本身的运动所产生的加速度会产生很大的干扰信号叠加在上述测量信号上，使得输出信号无法准确反映车模的倾角，尽管平滑滤波可以把波动滤掉，但是也会无法准确反映车模倾角变化，所以需要在加上一个陀螺仪检测角速度。2.3整车布局小车整体布局如图2.3所示1） 机械结构方面: 电路板紧贴车身，电池放在车身后轮子稍上，降低重心； 采用高强度、低质量的碳素杆作为电磁传感器支架； 简化支架，单杆伸出，粘上一个横放的轻杆作为电感的支撑； 电磁传感器引出电感，装在支架上，其他部分装在车身后面，可以尽量减轻支架的质量，使得重心尽量靠近车轮； 陀螺仪和加速度计模块装在车身中间，保持水平，使得检测回来的数据更加精确，并且使得左右转弯差异变小； 剪短不必要的连线，减轻车重。2）在硬件方面: 力求电路构成简单，稳定性高；传感器的安装可靠，外界对系统电路的干扰小。硬件主要设计思路如下：主电路板外形贴合车身的边沿，安装时尽量贴近车底盘，降低重心；电路板布线尽量采用单回路布线，并且将数字地与模拟地分离，并且在容易引入干扰的器件旁加入旁路电容；将容易产生静电干扰的部分，接入电路板的模拟地中。3）在软件方面：力求控制算法适合C车的特性，精确完成转向控制和速度，并具有较高的稳定性。采用K60作为控制核心，通过电感采集赛道信息，从而对电机进行控制；通过编码器反馈速度信息，实现对速度的控制。 图2.3 整车布局2.4结构框图根据两轮平衡小车设计方案，小车系统包括1） 角速度检测；2） 角加速度检测；3） 路径检测；4） 串口通信；5） 速度检测；6） 电机驱动。这几大模块按照设计组合在一起构成了一个完整的平衡小车控制系统具体系统的框架如图1.3所示。图2.4整体框架第三章 系统机械结构设计与实现3.1车模图3.1 c车车模结构车模型号：N286电机：RN260-CN 38-181303.2 车模车身简化及固定我们采用赛制规定的C车车模进行改装，将没有用的前轮及其支撑部件去掉，又将后轮悬挂缓冲支架去掉，从而简化车模，可以有更多空位安装需要的东西以及减轻车重。原来的车模后轮支架与底盘之间采用了活动连接方式，但为了保证车模直立的稳定性，需要这个车身变成刚体结构。所以我们用热熔胶在后轮支架与底盘之间的缝隙处进行粘接。但在跑的时候车身不稳，故改用废电路板将其固死，为防螺丝松掉，又用AB胶将缝隙粘死。整车如图3.2所示。图3.2简化加固后的车模3.3 陀螺仪及加速度计固定结构设计陀螺仪安装主要分为直立陀螺仪的安装及转向陀螺仪的安装。直立陀螺仪的安装共有三种选择：安装在顶部,正中和底部。经实际测试，陀螺仪安装在高处，容易受到微小抖动的干扰，影响到车身倾角的检测，而底部需要放电池及电路板等重物，因此我们选择了一个比较正中的位置进行安装。如图所示。而转向陀螺仪对高度并不是很敏感，因此取一个左右适中的位置即可。如图所示。图3.3.1 直立陀螺仪安装 图3.3.2转向陀螺仪安装3.4 路径传感器固定结构设计为了车身的稳定，车的重心越靠近车轮越好，所以我们采用了轻质量但高强度的碳素杆做成支架，并且只把电感引出去水平的套在支架前段，作为探路所用。如图3.4所示。 图3.4电磁传感器支架3.5 编码器的安装设计飞思卡尔c车车模具有双电机，为了达到更精准的控速，我们选择在两个电机分别安装编码器用来测速。在尽量保持车身重量较轻的前提下，我们应选用安装方便，较为轻便的编码器。由于光电码盘的精度不高，并且体积较大，不适用于c车的使用。我们使用了制作轻巧的300线的编码器，编码器可方便的安装在车轮上部，与马达齿轮相互咬合，来达到精确的测速。编码器的安装如图3.5所示。 图3.5 编码器安装3.6 主动轮松紧程度的调整在实际调试过程中，我们发现主动轮的松紧程度影响比较大，并且轮轴的有无影响也比较大。主动轮过松，会造成轮子与地盘支架之间有间隙，轮子会左右移动，对转弯性能影响较大，而轮子过紧则会造成轮子转动受阻，增加电机的负荷，不利于小车的速度及转弯控制。而去掉轮轴，则会造成轮子晃动，两个轮子不在同一直线，转弯不稳定。最终我们保持轮轴，并调整轮子的松紧度至轮子可自由转动且不会左右松动。第四章 硬件系统的设计与实现4.1 硬件设计总体方案智能车的硬件系统，主要包括控制电路，电源管理电路，电机驱动电路以及外围传感器构成，实现了信息采集与控制实现等功能。系统采用的主芯片是龙丘公司制作的MK60DN512VLL10P100管脚的核心板KinetisK60芯片，内已封装了包括看门狗等一系列外围电路。4.2 系统电路的设计与实现4.2.1 电源管理电路小车系统中有多个检测模块和驱动模块，需要不同的电源电压，所以电源管理部分要分部完成对各个模块的供电。系统存在着功率电路与数字电路，为了更好的隔离干扰，使系统的稳定性更高，所以在电源设计方面要考虑分隔开数字地与模拟地。当小车驱动电机在运动过程中，会在一定程度上拉低电池的电压。为了提高系统的性能，我们采用低压降的稳压芯片LM2940LM2940具有高转换效率，低静态电流等优点，适合本系统工作特点。因为K60需要3.3V供电，因此用LM1117-3.3搭了3.3V稳压电路。另外，电磁检测电路我们采用了稳压芯片7806，产生6V供电。本系统中，采用一路LM2940为无线模块等供电，一路7806为电磁检测模块供电，一路LM1117进行供电为单片机供电。6V供电如图4所示,5V供电如图4所示,3.3V供电如图4所示。图4 6V供电图4 5V供电图4 3.3V供电4.2.2 电机驱动电路今年C车车模我们采用了传统的用Mos管搭成的H桥全桥驱动，全桥驱动可以输出两路PWM，并且允许通过较大电流，驱动能力强，稳定性好，如图4.2.3.1所示。图4.2.3.1 电机驱动由于Mos管比较多，因此在电路板上排布比较密集，如图4.2.3.2所示。电路板上电机驱动排布4.2.3 计数器电路K60芯片只有一路低功耗脉冲累加通道,不能满足C车电路两路测速需求.如果不想占用资源而实现硬件计数,则需要用到DMA功能,通过读取DMA的循环计数器即可获得编码器脉冲值,从而用作速度计算，所以不需要特别的计数器，只需接个上拉电阻即可，如图4.2.4所示。图4.2.4 计数器电路4.2.4平衡传感器电路我们采用的陀螺仪跟加速度计是集成模块，陀螺仪电路分别如图4.2.5.1所示，加速度计电路如图4.2.5.2所示。图4 陀螺仪电路 图4加速度计电路4.2.5 路径检测电路路径检测分三个部分，谐振、放大、整流三个部分结合成一个电磁检测电路。具体电路如图4.2.6所示。路径检测电路4.3防静电处理由于KT板的特性，当智能小车在上面摩擦过后，会在赛道表面累积大量的静电。当小车在赛道上行驶的过程中，静电放电会对电路板的稳定性产生一定的干扰，严重时，引起单片机的复位和死机等问题。在空调室内的赛道，空气更加干燥，静电现象尤为严重。为了更好地排除外界静电对智能车系统的干扰，我们采取了以下措施：1） 因为小车底盘较低，底盘部分裸露的螺母会成为静电的释放源。我们采用绝缘胶布粘贴小车底盘，初步隔离静电2） 在电路设计上，通过使布线更加讲究合理，并且在电源和易受干扰的地方加入旁路电容，进一步减少静电的干扰。对地覆铜，对引入的干扰能更好的吸收。3）此时若依旧存在较大的静电干扰，那就要将小车上静电最强烈的地方接入电路板的地中，更好的吸收引入的干扰。在实际测试中，我们检测到编码器的支架和外壳部分产生的静电干扰最为严重，我们将其与电路板的模拟地线相连，有显著的改善第五章 软件系统的设计与实现5.1主程序5.1.1程序流程5.1.2 PWM输出包括两个子程序： 第一个程序：vconvoid vcon(double pwm) uint32 dPWM; dPWM=(uint32)pwm; if(dPWM9999) dPWM=9999; if(dPWM0) /电机1ftm0_pwm_change_duty(6,(uint32)(-dPWM);/控制占空比,占空比为0 010000ftm0_pwm_change_duty(4,0);/控制占空比,占空比为0 010000 /电机2ftm0_pwm_change_duty(5,(uint32)(-dPWM);/控制占空比,占空比为0 010000ftm0_pwm_change_duty(7,0);/控制占空比,占空比为0 010000 Else /电机1ftm0_pwm_change_duty(6,0);/控制占空比,占空比为0 010000ftm0_pwm_change_duty(4,(uint32)(dPWM);/控制占空比,占空比为0 010000 /电机2ftm0_pwm_change_duty(5,0);/控制占空比,占空比为0 010000ftm0_pwm_change_duty(7,(uint32)(dPWM);/控制占空比,占空比为0 010000 第二个程序：vcon_2motor void vcon_2motor(double Lpwm,double Rpwm) double RdPWM,LdPWM; RdPWM=Rpwm*10; LdPWM=Lpwm*10; /右边电机PWM限制 if(RdPWM9999) RdPWM=9999; /左边电机PWM限制 if(LdPWM9999) LdPWM=9999; /右边电机PWM输出 if(RdPWM0) /右电机ftm0_pwm_change_duty(7,(uint32)(-RdPWM);/控制占空比,占空比为0 010000ftm0_pwm_change_duty(5,0);/控制占空比,占空比为0 010000 Elseftm0_pwm_change_duty(7,0);/控制占空比,占空比为0 010000ftm0_pwm_change_duty(5,(uint32)(RdPWM);/控制占空比,占空比为0 010000 /左边电机PWM输出 if(LdPWM0)/左电机 /电机1ftm0_pwm_change_duty(6,(uint32)(-LdPWM);/控制占空比,占空比为0 010000ftm0_pwm_change_duty(4,0);/控制占空比,占空比为0 010000 Else /电机1ftm0_pwm_change_duty(6,0);/控制占空比,占空比为0 010000ftm0_pwm_change_duty(4,(uint32)(LdPWM);/控制占空比,占空比为0 010000 5.1.3 直立控制因为暂时找不到别的方法来平衡小车，所以只能靠两个电机来维持小车的平衡。根据陀螺仪以及加速度计返回来的数据进行积分，得到车身的角度和角速度，并根据积分数据，利用PD控制，5ms控制一下直立，使得车身得以保持平衡，作为速度控制的基础。5.1.4 速度控制小车的平衡是优先考虑的要素，所以为了不影响小车的平衡，我们不能频繁的改变速度，因此我们100ms才控制一次速度，并且使用增量式PID控制，即将需要的PWM分成20份，每5毫秒加一次，直至加完进行下一次速度控制。5.1.5 转弯控制车模方向控制是通过比较车模两个电感检波电压进行的，每5ms检查一次，然后采用比例算法，计算两个电感电压差除以两个电感电压之和所得到的数值在-1.0至1.0之间的比例，在乘以一个比例系数。5.2卡尔曼滤波卡尔曼滤波是一种高效率的递归滤波器， 它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。加速度计采样回来的数据会因为微小震动而产生毛刺，从而产生误差，因此用卡尔曼滤波结合陀螺仪输出的角加速度，把产生的误差降到最小。程序如下。void Kalman_Filter(double angle_m,double gyro_m)/gyro_m:gyro_measureangle0+=(gyro_m-q_bias) * dt;/先验估计Pdot0=Q_angle - P01 - P10;/ Pk- 先验估计误差协方差的微分Pdot1=- P11;Pdot2=- P11;Pdot3=Q_gyro;P00 += Pdot0 * dt;/ Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差P01 += Pdot1 * dt;P10 += Pdot2 * dt;P11 += Pdot3 * dt;angle_err = angle_m - angle0;/zk-先验估计PCt_0 = C_0 * P00;PCt_1 = C_0 * P10;E = R_angle + C_0 * PCt_0;K_0 = PCt_0 / E;/KkK_1 = PCt_1 / E;t_0 = PCt_0;t_1 = C_0 * P01;P00 -= K_0 * t_0;/后验估计误差协方差P01 -= K_0 * t_1;P10 -= K_1 * t_0;P11 -= K_1 * t_1;angle0+= K_0 * angle_err;/后验估计q_bias+= K_1 * angle_err;/后验估计angle_dot0 = gyro_m-q_bias;/输出值（后验估计）的微分 = 角速度5.3启动方式直立小车的启动与四轮车的启动有所不同,四轮车本身就具有相当高的稳定性, 只需设置定时开车程序即可.而直立小车启动需要确保静止没问题,然后再启动.在调试过程中,我们发现主要有两种开车方式:第一种是小车直立后手推启动式,第二种是小车直立后受触发启动式.对于第一种方法,小车被推动后,倾角和速度都会发生变化,我们可以通过检测小车的倾角或者速度,设置一个阀值,触发小车启动控速程序,进而实现小车的启动.这种方法的优点是,小车的启动速度快,若起跑线后是一条长直道,更可大大提高小车的整体成绩.而缺点是,推的力度不同导致小车的稳定性不同,很有可能每圈跑出来的成绩不一样.对于第二种方法,小车直立后,通过轻触小车上面的行程开关,使得小车自己启动.这种方法的优点是,小车每次开头跑的情况基本一样,稳定性较好.缺点是,小车的启动速度略逊于第一种.综合考虑,我们认为小车的稳定性比较重要,所遇采取第二种启动方式.第二种启动法式所用到的行程开关如图5.3所示:图5.3行程开关第六章 开发工具与系统调试6.1 开发工具使用了CodeWarrior Development Studio for Microcontroller v10.1集成开发环境。图6.1.1是软件的安装界面，图6.1.2是烧录程序的界面。图6.1.1 安装界面 烧录界面6.2 无线通信模块在调试过程中，为了能够实时获取智能车在运动过程中的一些参数，以及避免智能车在尝试各种算法时冲出赛道而发生意外，我们使用了无线通信模块传输数据。为此，我们设计了智能车监控软件和遥控器。车载无线模块如图所示，能够实时收发数据。图为我们设计的遥控器，用于控制智能车的启停，保护智能车。监控软件如图所示，可以收集智能车的实时速度和位置偏差等数据，我们能够更加直观方便地观察智能车的状态，方便调试。图车载无线模块图遥控器图监控界面第七章 车辆参数车辆参数如表7.1所示 表7.1 车辆参数车模质量约900克车模几何尺寸长宽高：35cm17cm28cm传感器个数电磁传感器1对，陀螺仪2个，加速度计1个，编码器2个电机个数2个芯片个数IRF49054个，IRF32054个，LM78061个，LM29401个，LM11171个，KinetisK60芯片1个，45201个赛道检测精度5mm赛道检测频率200次每秒总电容量约1780微法总功率约10W第八章 总结在整个赛车设计制作过程中，我们收获良多，除了合理运用现有的理论知识，我们还要通过课外的扩展，不断地学习新知识，譬如陀螺仪的应用、加速度计的应用、卡尔曼滤波。除此之外，我们还要将我们所学应用在实际当中，不断地端正自身的态度，锻炼实践能力，提高发现问题、解决问题的能力，加强团队合作的意识。在参加完赛区比赛后，我们发现自己跟兄弟学校的不足之处，主要在于传感支架比较重，不够简洁、长直道加速后转弯摩擦不够强，不足以提供转弯向心力、轮子会打滑等等。这些都提醒我们，以后要注意提高小车机械结构上的改造能力，要完善自身的理论基础。致谢感谢飞思卡尔半导体公司，感谢教育部，自动化专业教学指导分委会为我们全国高校的学子，提供了一个锻炼团队合作和创新能力的竞赛平台。使得我们在备赛过程中，收获颇丰。感谢中山大学信息科学与技术学院的领导与老师对比赛的关注，在物资上提供大力支持，使我们能专心做事。感谢中山大学实验中心的领导与老师，为我们提供了良好的调试场地与齐全的工具设备。感谢指导老师们的支持和帮助，有了他们的指导，我们才能取得现在的成绩。参考文献【1】 李仕伯.马旭.卓晴.基于磁场检测的寻线小车传感器布局研究，清华大学，2009.12【2】 竞赛组秘书处，路径检测设计参考方案，2010.1【3】 卓晴 .基于磁场检测的寻线小车传感器布局研究【J】.清华大学.2007【4】 竞赛组秘书处，直立行走的车模（早期版本），2011.11【5】 竞赛组秘书处，直立行走的车模（后期版本）2011.11【6】 竞赛组秘书处，直立行走的车模（大圆圈），2011.11【7】 竞赛组秘书处，电磁组参考设计方案_最终版本new,2011.12【8】 竞赛组秘书处，电磁组直立车模参数整定与调试指南手册.part1 2012.02【9】 竞赛组秘书处，电磁组直立车模参数整定与调试指南手册.part22012.02【10】 竞赛组秘书处，电磁组参考设计方案第二版2012.03【11】 竞赛组秘书处，新版电磁组参考设计方案第二版2012.03【12】 第六届杭州电子科技大学钱江1号的技术报告，2011.08【13】 第五届中山大学鹰之队的技术报告，2010.08附录A.系统电路图附录B程序Main.c/* * main implementation: use this C sample to create your own application * */#include #includederivative.h/* include peripheral declarations */#includeincludes.h#includemath.h#include#define PIT0 0#define PIT1 1#define PIT2 2#define PIT3 3euint8 data1;uint8 data2;uint8 data3;uint8 data4;uint8 data_rec;uint8 data_rec1;uint8 data_rec2;uint8 data_rec3;/*/全局变量声明externint periph_clk_khz;#define ADchannel 16uint8 cycle_flags = 0;uint16 result0A,result0B,result1A,result1B;uint8 isr_flag=0;/PIT中断标志,为1时发生中段uint32 run_counter;#define OS_SCI1_RATE 57600 /数据发送的波特率为57600 #define OS_BUS_CLK80/总线频率为80MHzint car_motion_period=20;/车调速的周期int dead_value=0;/死区电压double angle10, angle_dot10;/外部需要引用的变量/*全局变量声明*int top_key_num=0,j;int key_start_flag=0;unsignedchar sendjiaodu;int send_angle;int send_jishu=0;int beepflag=0; int sudu20;/ 记录车的速度（路程）int sudu_L20;unsignedchar data_rec,data_rec1,data_rec2,data_rec3;/接收数据int vol1510;/ 八个传感器，记录10次历史值 float pian,pian2,pian3; short delv1,delv2,delv3;long suibianjishu=0;/suibianjishu,随便计数,检测时间.short d_pwm2 = 0,0;float a,he,cha;float y;float y_value; float d_y4;float t;float value1,value2;int PWMstart=0;int set_v = 0;double e1,e2;char isStart = 0;int xianzhi;int jiaodu=0;double acc10;double w10;double utpwm;int Speed_period;float Lcountspeed=0;float Rcountspeed=0;float zonglucheng;int PWM_total;int display_Vcon_pwm;int delv1_R,delv2_R,delv3_R;short delv1_L,delv2_L,delv3_L;int speed_period;float R_PWM=0;int L_PWM=0;unsignedchar SCI1_flag; /接收函数的标志位unsignedchar reveive_data;/接收到的数据int RValue,LValue;int send_send_period;int turn_pwm;int stop_flag,forward_flag,turn_flag,send_flag;int sudu_rec=860;/控速相关float R_PPWM,newpwm,oldpwm,deltapwm;float vset,vstart;int start_count=0;int endl=0;int dazhao;int swit20;/拨码开关int a_switch34;int sudu_receive=0;int stand_key_count=0;/静止开关检测延时int i=0;uint16 ADValue,ADValue1,ADValue2,ADValue3;int speed_R;int speed_L;int beepfirst_flag=0;float piancha_zhongzhi=0;int startbeepflag=0;int goout=0;char turn_beep=0;float lingdian=0;int last_beep=0;int boma_flag=0;unsignedlong L=0;unsignedlong R=0;float er=0;/E为历史平均值char bflag=0;double lingdian_jiaozheng=0;double lingdian_sum=0;int lingdian_count=700;int testzhouqi=0;float max;float min;/float lishi2=0;float guaidian81;char tubian=0;int q_zheng=0;int q_fu=0;int h_zheng=0;int h_fu=0;unsignedlong pre=0;unsignedlong now=0;/方差检测double dy_test400=0;double dy_sum=0;double dy_ave=0;double dy_fangcha;double dy_ans;int gengxin=0;int dy_zhouqi=30;int dy_jiange=1;int wan_cd_time=0;int wan=0;int sudu_overbeep=0;/*/*关于利用增量式计算左右电机的PWM以及PWM输出的程序*/voidvcon_R(int vset) float dPWM,PPWM;/PPWM是为了可以与静止的utpwm相互结合而成的一个中间变量 delv3_R=delv2_R; delv2_R=delv1_R; delv1_R=vset-(Rcountspeed+Lcountspeed)/2; dPWM=3*(delv1_R-delv2_R)+0*delv1_R+1.2*(delv1_R+delv3_R-2*delv2_R);/增量式PID控速 R_PWM+=dPWM;/ if(R_PWM0)/R_PWM=0; PPWM=R_PWM; R_PPWM=-PPWM; oldpwm=newpwm; newpwm=R_PPWM; voidswitches() char switchcount;char luchengcount;char houchengsuducount;/char lingdiancount;switchcount=15-(8*swit3+4*swit2+2*swit1+swit0);/开始设定速度luchengcount=15-(8*swit7+4*swit6+2*swit5+swit4);/路程houchengsuducount=15-(8*swit11+4*swit10+2*swit9+swit8);/后面速度/lingdiancount=7-(4*swit15+2*swit14+1*swit13);/零点if(boma_flag=0)if(switchcount=0)/1111 15 sudu_rec=860;elseif(switchcount=1)/1110 14sudu_rec=920;elseif(switchcount=2)/1101 13sudu_rec=980;elseif(switchcount=3)/1100 12sudu_rec=1000;elseif(switchcount=4)/1011 11sudu_rec=1040;elseif(switchcount=5)/1010 10sudu_rec=1080;elseif(switchcount=6)/1001 9sudu_rec=1120;elseif(switchcount=7)/1000 8sudu_rec=1160;elseif(switchcount=8)/0111 7sudu_rec=1200;elseif(switchcount=9)/0110 6sudu_rec=1240;elseif(switchcount=10)/0101 5sudu_rec=1260;elseif(switchcount=11)/0100 4sudu_rec=1280;elseif(switchcount=12)/0011 3sudu_rec=1300;elseif(switchcount=13)/0010 2sudu_rec=1320;elseif(switchcount=14)/0001 1sudu_rec=1380;elseif(switchcount=15)/0000 0 鏈蹇?sudu_rec=1440;elseif(houchengsuducount=0)/1111 15 鏈鎱? sudu_rec=860;elseif(houchengsuducount=1)/1110 14sudu_rec=920;elseif(houchengsuducount=2)/1101 13sudu_rec=980;elseif(houchengsuducount=3)/1100 12sudu_rec=1000;elseif(houchengsuducount=4)/1011 11sudu_rec=1040;elseif(houchengsuducount=5)/1010 10sudu_rec=1080;elseif(houchengsuducount=6)/1001 9sudu_rec=1120;elseif(houchengsuducount=7)/1000 8sudu_rec=1160;elseif(houchengsuducount=8)/0111 7sudu_rec=1200;elseif(houchengsuducount=9)/0110 6sudu_rec=1240;elseif(houchengsuducount=10)/0101 5sudu_rec=1260;elseif(houchengsuducount=11)/0100 4sudu_rec=1280;elseif(houchengsuducount=12)/0011 3sudu_rec=1300;elseif(houchengsuducount=13)/0010 2sudu_rec=1320;elseif(houchengsuducount=14)/0001 1sudu_rec=1380;elseif(houchengsuducount=15)/0000 0 鏈蹇?sudu_rec=1440;if(luchengcount0)/鍗婄鍑忛?if(luchengcount=1 & (L+R)/2)77400)/77400 鍗婄15mboma_flag=1;last_beep=1; elseif(luchengcount=2 & (L+R)/2)103200)/103200 鍗婄20mboma_flag=1;last_beep=1; elseif(luchengcount=3 & (L+R)/2)129000)/129000 鍗婄25mboma_flag=1;last_beep=1; elseif(luchengcount=4 & (L+R)/2)154800)/154800 鍗婄30mboma_flag=1;last_beep=1;elseif(luchengcount=5 & (L+R)/2)180600)/180600 鍗婄35mboma_flag=1;last_beep=1;elseif(luchengcount=6 & (L+R)/2)206400)/206400 鍗婄40mboma_flag=1;last_beep=1;elseif(luchengcount=7 & (L+R)/2)232200)/232200 鍗婄45mboma_flag=1;last_beep=1;elseif(luchengcount=8 & (L+R)/2)258000)/258000 鍗婄50mboma_flag=1;last_beep=1;elseif(luchengcount=9 & (L+R)/2)283800)/283800 鍗婄55mboma_flag=1;last_beep=1; elseif(luchengcount=10 & (L+R)/2)309600)/309600 鍗婄60mboma_flag=1;last_beep=1; elseif(luchengcount=11 & (L+R)/2)335400)/335400 鍗婄65mboma_flag=1;last_beep=1; elseif(luchengcount=12 & (L+R)/2)361200)/361200 鍗婄70mboma_flag=1;last_beep=1;elseif(luchengcount=13 & (L+R)/2)387000)/387000鍗婄75mboma_flag=1;last_beep=1;elseif(luchengcount=14 & (L+R)/2)412800)/412800 鍗婄80mboma_flag=1;last_beep=1;elseif(luchengcount=15 & (L+R)/2)438600)/438600 鍗婄85mboma_flag=1;last_beep=1;/*if(swit12=0)if(lingdiancount=0)lingdian=0;else if(lingdiancount=1)lingdian=0.5;else if(lingdiancount=2)lingdian=1;else if(lingdiancount=3)lingdian=1.5;else if(lingdiancount=4)lingdian=2;else if(lingdiancount=5)lingdian=2.5;else if(lingdiancount=6)lingdian=3;else if (lingdiancount=7)lingdian=3.5;else if(lingdiancount=0)lingdian=0;else if(lingdiancount=1)lingdian=-0.5;else if(lingdiancount=2)lingdian=-1;else if(lingdiancount=3)lingdian=-1.5;else if(lingdiancount=4)lingdian=-2;else if(lingdiancount=5)lingdian=-2.5;else if(lingdiancount=6)lingdian=-3;else if (lingdiancount=7)lingdian=-3.5;*/voidvcon_start(int vset) if(start_count8) vcon_R(-720); start_count+; startbeepflag=1;if(start_count11) vcon_R(-775); start_count+; startbeepflag=1;if(start_count14) vcon_R(-815); start_count+; startbeepflag=1;elseif(start_count860) vcon_R(-860); start_count+; startbeepflag=1;elseif(start_count900) vcon_R(-900); start_count+; startbeepflag=1;elseif(start_count930) vcon_R(-935); start_count+; startbeepflag=1;elseif(start_count975) vcon_R(-975);
展开阅读全文