关于直立平衡和智能循迹的小车控制系统结构设计 36页

'关于直立平衡和智能循迹的小车控制系统结构设计第一章引言1.1智能车的发展历史智能车的研究始于20世纪50年代初美国BarrettElectric公司开发出的世界上第一台自动引导车辆系统(AutomatedGuidedVehicleSystem,AGVS)。1974年，瑞典的VolvoKalmar轿车装配工厂与Schiinder-Digitron公司合作，研制出一种可装载轿车车体的AGVS，并由多台该种AGVS组成了汽车装配线，从而取消了传统应用的拖车及叉车等运输工具。20世纪80年代，伴随着与机器人技术密集相关的计算机、电子通信技术的飞速发展，国外掀起智能机器人研究热潮，其中各种具有广泛应用前景和军用价值的移动式机器人受到西方各国的普遍关注。全国大学生智能汽车竞赛是在统一汽车模型平台上，使用飞思卡尔半导体公司的8位、16位微控制器作为核心控制模块，通过增加道路传感器、设计电机驱动电路、编写相应软件以及装配模型车，制作一个能够自主识别道路的模型汽车，按照规定路线行进，以完成时间最短者为优胜。该竞赛涵盖了控制、模式识别、传感技术、电子、电气、计算机、机械等多个学科交叉的大学生课外科技创意性比赛。1.2智能车竞赛内容和主要研究全国大学生智能汽车竞赛已经成功举办了七届，比赛规模不断扩大、比赛成绩不断提高。该竞赛涵盖了控制、模式识别、传感技术、电子、电气、计算机、机械等多个学科交叉的大学生课外科技创意性比赛。通过比赛培养大学生的综合知识运用能力、基本工程实践能力和创新意识，激发大学生从事科学研究与探索的兴趣和潜能,倡导理论联系实际、求真务实的学风和团队协作的人文精神。赛车采用飞思卡尔32位微控制器MK60作为核心控制单元，由学生自主构思控制方案及系统设计，包括传感器信号采集处理、控制算法及执行、动力电机驱动、直立转向控制等，完成智能车工程制作及调试。为完成本系统，主要做得内容分为以下几部分：（1）对车模机械部分进行详细的分析和调整；36 （2）分析控制电路各模块的要求，计算出各模块器件的参数，设计完成硬件控制电路；（3）通过CCD传感器采集赛道信息；（4）对数据进行分析，设计控制算法并编写控制程序。第一章总体方案设计赛车的性能主要由机械结构、硬件和软件三部分决定。机械结构是赛车能够行驶的根本，赛车有了一定的机械结构，再加上相应的硬件和软件，就构成了一个完整的系统。2.1机械方面的总体设计智能车的机械调整作为最基础的硬件其重要性是首位的，由于本届的车模只用后轮保持直立行走，车模没有舵机控制，所以机械结构比较简单，可以改动的地方不多，以往有关于调整车模的经验对于今年的比赛来说也是至关重要的，所以我们首先仔细阅读了往年的技术报告作为参考，我们车模的机械调整主要分为：编码器的安装；CCD支架的安装；CCD位置的调整，主控板驱动电路板的安装固定；加速度陀螺仪板的安装调整。2.2硬件总体设计1）主控使用飞思卡尔公司的144引脚的Mk60芯片，因为其引脚IO多，可以控制的外围较多的外围设备。系统控制结构图如图2.1所示。图2.1系统控制结构图2）电源模块，通过7.236 伏的总电源根据不同的模块采用不同的稳压方案，用到了线性稳压，由于7.2V较低我们采用低压差线性稳压。单片机3.3V供电用线性稳压；CCD采用5V供电，采用低压差线性稳压；3）电机驱动，由于电机电流较大，工作电流3-5A，所以电机驱动模块采用BTS7970和H桥配合4）输入模块采用按键调节5）通过CCD采集赛道上128个像素点，对于不同点的值进行处理，可以检测赛道的情况6）通过三轴加速度和陀螺仪对角度实时采集处理，使小车保持直立以及加减速此外还有码盘等测速器件。2.3软件总体设计软件控制分为直立控制、速度控制和转向控制。其中直立闭环控制如图2.2所示，速度闭环控制如图2.3所示，转向闭环如图2.4所示。图2.2角度控制图2.3速度控制图2.4转向控制软件除了三个闭环还包括各种控制策略。策略应用简单的根据寻线来改变速度值，从而良好的解决十字交叉、起跑线、障碍等问题，保证赛车的全局稳定运行。第三章赛车车体机械结构设计3.1编码器的安装作为反馈的提供者，编码器安装的精度直接影响到测速的准确和对速度控制的准确度。今年的车模所留下的安装编码器的空间有限，所以对编码器的36 选型提出了更高的要求。最终我们选择了龙邱产的光电编码盘100p/r的增量型编码器，此光电编码器比一般的编码器灵敏度精确度要高，但是由于没有齿轮传动部分，所以不便于安装，鉴于以上情况，我们将螺丝柱粘在电机齿轮上，在螺丝柱上固定编码盘，然后在车架上固定红外计数器，来记录码盘转速。编码器的安装如图3.1所示。图3.1编码器的安装3.2CCD支架的安装在测试CCD时，发现CCD视角较小，最佳可视距离较大，但车模的高度不够高，为了让CCD扫描视线和赛道夹角尽量的大点，使CCD处于最佳采集位置，给车模加装了碳纤竿，加高了CCD，效果比较理想。CCD安装如图3.2所示。36 图3.2CCD的安装3.3主控驱动电路板的安装固定由于车底盘空间较小，而且空间比较复杂，如果想完全放到底盘上难度较大，所以将电机主控电路和驱动电路分开放置，便于合理利用小车空间安装。将其放到了电机的上面。虽然提高了重心但经过再三观察，由于电路板太大，再加上降低重心，只能将其放到电机上方，不过接口等方便明了，提高了我们的调试效率。主控驱动电路的PCB如图3.3所示。图3.3主控驱动电路的PCB图36 3.4加速度陀螺仪板的安装加速度陀螺仪主要用于检测小车倾角，放置的位置对于所测数据有很大的影响，所以放置时要将其放置水平。放置位置如图3.4所示。图3.4陀螺仪板的安装36 第四章赛车硬件系统设计4.1核心板的设计32位Kinetis系列列单片机MK60是硬件系统的核心部分，用于赛车的整体控制，包括信息的采集处理和输出，其最小系统如图4.1所示。图4.1MK60最小系统4.2电源模块的设计系统的正常工作要有稳定可靠的电源保障。系统中需要的电压值主要有：7V，5V,3.3三种。7V电压主要为电池接入口处，一路为直接为电机驱动供电，另一路经过两片LM2940芯片分别产生两个5V电压值。5V电压有两路，其中一路为光电码盘供电，另一路为1117供电，为主控、CCD、三轴加速度陀螺仪提供3.3V电压，。这种两路5V供电的设计，可以减小直流电机开启瞬间电压变化影响，避免了单片机因电压值减小引起的复位及CCD接受电压变化。电源电路如图4.2所示。36 图4.2电源电路4.3电机驱动模块的设计电机驱动模块采用BTS7970和H桥配合：74HC244驱动MOS管，MOS管驱动电机，这样不但满足电机驱动需求，而且驱动散热好，避免了温漂带来的影响。电机驱动通过IN1和IN2引脚输入PWM波形，以此调节驱动OUT1，OUT2引脚输出电压，调节电机转动速度，以及正反转和拐弯，达到不同的控制效果。电机驱动板如图4.3所示。图4.3电机驱动板4.4输入模块的设计输入采用按键，加防抖电容，编程简单，通过给按键设置不同的功能，用于设置不同的参数，以此来更改智能车行车模式，适应各种情况。4.5光电编码器的安装光电编码器，反馈当前速度，使整个系统形成一个闭环系统。我们应用的是龙邱的光电编码器，由于两路电机分别控制，所以要用两个光电编码器。该编码器电源电压为3.3V--5V，接线方式：红色电源线，黑色接地线，两根黄色为信号线。我们一组黄色线接单片机P66和P81口。另一组黄色信号线接P73和P88口捕捉信号。4.6CCD的安装CCD应采用官方规定的TexasAdvancedOptoelectronicSolution公司的TSL1401系列的线性CCD。从龙邱和蓝宙购买的CCD36 由于都已经做成了成品，只需引出接口接到单片机就可以了，使用方便，拍摄的线阵还是比较清晰的。本次我们用的CCD接口如图4.4所示。图4.4CCD接口电路第五章系统软件设计5.1寻起跑线的软件设计由于CCD可以识别黑色赛道线，通过对比可以得到黑色线的宽度，以及位置，由此情况来检测起跑线。当CCD检测到起跑线部分时，若按照正常的检测赛道算法此时得到的左右偏差近似相等且都很小。左右偏差相等能保证车在运行方向上不会出现抖动现象，然后抓住其都很小这一特征，再加上方向控制很小、CCD检测到左右各有特定的10个点为黑等条件。当同事满足时则认为赛车检测到了起跑线，运行起跑线处理程序。采用2个CCD传感器对赛道进行信息采集，上下CCD对赛道的可视距离不同，上边的CCD可视距离近，主要进行路况检测和转向控制；下边的CCD可视距离远，主要进行赛道类型判断和加减速判断。当远距CCD检测到弯道或障碍时，可以及时提前减速，安全通过，当遇到直道时，可以加速通过。同时双CCD可以加大赛道起跑线的检测率。两个CCD采用不同的阈值和阈值算法，采集和处理同时进行互不影响。大大加强了整车的稳定运行。5.2速度控制策略5.2.1速度PID控制速度控制采用增量式PID算法，所谓增量式PID是指数字控制器的输出只是控制量的增量Δuk.。当执行机构需要的控制量是增量，而不是位置量的绝对数值时，可以使用增量式PID控制算法进行控制。36 其中5.2.2速度改变策略为了使得小车跑的平均速度最大，不能在全程使用一个速度。在转弯的时候应当适当的减速，在直道的时候加速。怎么识别直道和弯道。我们可以根据CCD采集赛道黑白边界不同来识别，从而及时增减速度。Speed_Curr=(Right_Speed+Left_Speed)/2;速度值等于左轮和右轮的速度值得平均值。5.2.3小车转向的算法实现直立车的转向是通过两个轮子的转速差来实现的，故控制转向就是控制两个轮子的速度差。ZUO=ccd_move_aver_filt(DL_DB,Dist_Left,DIRE_NC);YOU=ccd_move_aver_filt(DR_DB,Dist_Right,DIRE_NC);ee=(int)(ZUO-YOU);偏差经过PD运算后得到转向控制的改变量。Dire_Out=(ee*DIRE_P+gyro_hor*DIRE_D)*0.1;5.3障碍识别策略在小车行进过程中，CCD不断地进行检测，当CCD采集到的数据，全部是黑色部分时，就判断为障碍，从而及时减速，安全通过。5.4坡度识别策略车在上坡的时候，CCD36 采集数据会出现盲区，数据采集不稳定，通过处理将其按直道行走，并通过三轴加速度计和陀螺仪检测其下坡，从而减速行驶。5.5起跑线识别策略由于CCD的前瞻较大,通过CCD采集黑道的情况来作为起跑线的识别，当检测到中间有间断的黑道时起跑线如图所示，CCD会看到黑白间隔的以此进行判定。起跑线示意图如图5.1所示。图5.1起跑线示意图5.6抗干扰性滤波算法对于CCD传感器，跑道外的干扰不可避免。为了避免干扰，我们减少了扫描的宽度。即下一时刻的扫描范围是以这一时刻的寻线结果有关。第六章开发工具及调试说明6.1开发工具的说明程序的开发是IAR下进行的，包括源程序的编写、编译和链接，并最终生成可执行文件。IAR是IARSystems公司为ARM微处理器开发的一个集成开发环境(下面简称IAREWARM)。比较其他的ARM开发环境，IAREWARM具有入门容易、使用方便和代码紧凑等特点EWARM中包含一个全软件的模拟程序(simulator)。用户不需要任何硬件支持就可以模拟各种ARM内核、外部设备甚至中断的软件运行环境。从中可以了解和评估IAREWARM的功能和使用方。6.2调试过程主要是用无线串口，将速度值发到监控板上边，在监控版上实时观看速度的情况，其界面如图6.1所示。36 图6.1系统监控界面对CCD数据的观察，用蓝宙电子科技公司的CCDView软件，如图6.2所示。图6.2CCDView软件截图硬件电路的制作采用的是AltiumDesigner6v6.7，v6.8，Summer08电子电路原理图，PCB图设计环境。第七章智能车的主要参数本智能车的外形参数如表7.1所示。36 表7.1智能车外形参数项目参数路径检测方法（赛题组）光电检测车模几何尺寸（长、宽、高）（毫米）162*248*475mm车模轴距/轮距（毫米）85mm/162mm车模平均电流（匀速行驶）(毫安)1100mA电路电容总量（微法）200uf传感器种类及个数4种6个新增加伺服电机个数0个赛道信息检测空间精度（毫米）15mm赛道信息检测频率（次/秒）200次/秒主要集成电路种类/数量电源电路、电机驱动电路、MCU最小系统、CCD传感器一个,陀螺仪，加速度计等车模重量（带有电池）（千克）1.5Kg第八章结论本报告详细介绍了我们为第八届全国大学生智能车比赛所做的准备及系统设计方案，包括激光传感器的设计思想、舵机转角方案以及速度控制策略等。分析整个系统模型，我们在车模硬件和软件上都有所创新。系统上主要特色有：机械方面：分析车体前后轮的机械特点，综合考虑程序的整体效果，确定适合的角度；经过实际测验传感器的各种排列，比较效果后，确定了CCD的高度和视角。电路方面：单片机主控板是我们自己设计，能够满足应用要求及合理的减少所占空间。主板以模块形式分类：MK60主控模块、电源模块、电机驱动模块、传感器模块、速度检测模块、辅助模块和起连接作用的主板等。算法方面：程序采用C语言编程，利用IAR开发工具调试，经过小组的不断讨论、改进，终于确定了比较稳定精确的方案。这套方案可以结合赛道的各种路况信息调整车体速度和转角，保证最短时间跑完全程。36 总观我们车模制作的过程，还有几方面有待改进的地方：单片最小系统版引脚全部引出，但实际应用没有全部用到，为减少其所占面积，可以只指引出应用到的引脚；传感器前瞻有待提高，CCD仍可以有更大前瞻，有利于系统的整体设计。总结整个比赛的过程，我们在各个环节都付出了最大的努力，做了最好的准备。比赛前期精心制作机械结构，调试硬件电路，学习软件知识；后期不分昼夜的进行实际调试。我们坚信，在努力的付出后一定会有回报。通过此次比赛，不仅锻炼了我们的软硬件知识，而且培养了我们的创新能力和团队精神。这份经历将是我们人生的宝贵财富。36 参考文献[1]邵贝贝.嵌入式实时操作系统[LC／OS-Ⅱ(第2版)[M].北京．清华大学出版社．2004[2]邵贝贝.单片机嵌入式应用的在线开发方法[M]．北京．清华大学出版社．2004[3]王晓明.电动机的单片机控制[M]．北京.北京航空航天大学出版社．2002[4]童诗白，华成英．模拟电子技术基础[M]．北京.高等教育出版社．2000[5]沈长生．常用电子元器件使用一读通[M]．北京.人民邮电出版社．2004[6]宗光华．机器人的创意设计与实践[M]．北京.北京航空航天大学出版社．2004[7]张伟等．ProtelDXP高级应用[M]．北京.人民邮电出版社．2002附录36 源程序代码：附录A：#include"common.h"#include"outputdata.h"#defineGPIO_PIN(x)(((1)<<(x&GPIO_PIN_MASK)))////////////////////////////////////////////////////////////函数名定义区voidDisplay_Init(void);voidAngle_Out();voidSpeed_Calc(void);//速度值更新flag_encodervoidSpeed_Out(void);//速度值分步输出Speed_TimeENCODER_PERIODvoidDire_Calc(void);//方向值更新voidDire_Out(void);//方向值分步输出charUART_Change(void);voidMotor_Out();voidcall_init();voidUp_Load();uint8Motor_Cnt=0;charANGLE_ADJ_ON=1,//角度调节开关SPEED_ADJ_ON=1,//速度调节开关DIRE_ADJ_ON=1,//方向控制开关DIRE_ADJ_ON_MASK=1,WIRELESS_ON=1;//zigbee无线监控开关intAngle_P=145,Angle_P_MASK=145,//角度控制P-180180Angle_D=1300,Angle_D_MASK=1300;//角度控制D-4001400charSPEED_M=1;floatSPEED_P=0,//速度控制P-90真实值SPEED_P_PREV=0,SPEED_P_MASK=50,//设定值SPEED_I=10,//速度控制I-18真实值SPEED_I_PREV=0,SPEED_I_MASK=400;//设定值intSPEED_SET=0,//速度设定值3036 SPEED_SET_MASK=52;//48,54floatSPEED_I_RANGE=300;floatDIRE_P=60,DIRE_P_PREV=0,DIRE_P_MASK=60,DIRE_D=60,DIRE_D_PREV=0,DIRE_D_MASK=600;charControl_Count=0;//中断协调各个时间的运行内容intSPEED_CTRL_n=0,//速度加权Speed_Time=0,//更新时间计数--速度Dire_Time=0,//更新时间计时--方向DIRE_CTRL_n=0;//方向加权floatAngle_M_Out=0,//角度控制量Angle_M_Out_Prev=0,Angle_M_Out_Curr=0,Left_Speed=0,//左轮速度Right_Speed=0,//右轮速度Speed_Curr=0,//当前速度值Speed_Prev=0,//上时刻速度值Speed_Error_Prev=0,//上时刻速度误差Speed_Error_Curr=0,//当前速度误差Speed_Error=0,Speed_Int=0,//速度误差积分值Speed_M_Out=0,//100MS速度控制输出值Speed_M_Out_Prev=0,Speed_M_Out_Curr=0,Speed_Out_Old=0,//上时刻速度输出Speed_Out_New=0,//当前速度输出Speed_Per_Out=0;//速度平均输出volatileintThreshold=0,Lock_Threshold=6,Threshold_Ratio=140,ee=0,Dist_Left=0,//左线为基准的偏差Dist_Left_Prev=0,Dist_Right=0,//右线为基准的偏差36 Dist_Right_Prev=0;volatilefloatStartLine_Threshold;volatilefloatCCD1_Threshold;floatDire_M_Out=0,//方向输出Dire_M_Out_Prev=0,Dire_M_Out_Curr=0,Dire_Out_Old=0,Dire_Out_New=0,Dire_Per_Out=0;//方向平均输出;uint16count=0,black_count=0,black_count_mask=400,start_count=0,//起跑线延时n个循环周期TIME_INIT=0,TIME_UPRIGHT=4000,//直立时间TIME_ACCE=1000;//加速时间volatilecharflag_blackcount=0,flag_black=0,flag_init=0,//正式运行前初始化开关flag_gyro=0,flag_speed_o=0,flag_dma_send=0,flag_dma_recv=0,flag_upload=0,flag_match=0;uint16count_S_i=0;charmode=1,//B3_cnt=1,flag_run=0,//控制电机运行标志flag_button=0;//按键次数标志uint16flag_angle_updatacnt=0;uint8flag_ooooo=0;//按键模式标记/////////////////////////////////////////////////////////////内核时钟externintcore_clk_khz,//内核时钟*KHZcore_clk_mhz,//内核时钟*MHZ36 periph_clk_khz;//外围器件时钟*KHZ/////////////////////////////////////////////////////////////陀螺仪intgyro_temp=0,//获得偏移量gyro_time=0,gyro_temp_hor;//水平方向偏移量longgyro_off_add;//偏移量的确定floatgyro_indv=0,//纯粹陀螺仪积分角度值gyro=0,//角速度值gyro_hor=0,//水平方向AngleSigma=0,//融合后角度值官网滤波Angle_1=0,//算法1角度值Angle_2=0;//算法2角度值互补滤波/////////////////////////////////////////////////////////////PWM变量区intPwm_Left=PWM_INIT,Pwm_Right=PWM_INIT,Pwm_Left_Prev,Pwm_Right_Prev,Pwm_Left_Curr,Pwm_Right_Curr;/////////////////////////////////////////////////////////////FTM变量区uint16TIM=0;//MS计时寄存器uint16TIM2=0;/////////////////////////////////////////////////////////////编码器测速变量intL_QD=0,L_QD_PREV=0,R_QD=0,R_QD_PREV=0;floatL_QD_Temp[QD_NC]={0},//进行平均滤波算法的暂存器R_QD_Temp[QD_NC]={0};/////////////////////////////////////////////////////////////AD变量区intad_offset[4];//偏移采样值寄存器longad_avercal[4];//采样值-偏移存放区intLock_Acce=199,//加速度计偏移量Lock_Gyro=415;//陀螺仪偏移量/////////////////////////////////////////////////////////////otheruint8i,j;intupload_count=0;36 intDL_DB[DIRE_NC]={0},//0-9存放各个时刻值10存放当前的误差值DR_DB[DIRE_NC]={0};intZUO=0,ZUO_1=0,YOU=0,YOU_1=0;uint8get_pixel[150]={0,132,0,0,0,0},send_pixel[32]={0xfd,0x19,0x00,0x00,//字符变量0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};uint8get_pixel1[150]={0,132,0,0,0,0},//volatilesend_pixel1[32]={0xfd,0x19,0x00,0x00,//字符变量0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};volatileuint8startline[32]={0xfd,0x19,0x00,0x00,//字符变量0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};volatileuint8startline_count,flag_startline=0;volatileunsignedintglobal_time;chartemp[15],//显示单元Rec[21]={0},Rcnt=0;/********************************************************************//********************************************************************///主函数区/********************************************************************//********************************************************************//********************************************************************/voidmain(void){All_Init();uart_init(UART3_BASE_PTR,periph_clk_khz,115200);PORTE_PCR4=PORT_PCR_MUX(0x3);//UART3isalt3functionforthispinPORTE_PCR5=PORT_PCR_MUX(0x3);//UART3isalt3functionforthispin36 UART3_eDMA_RecInit(2);myDMA_Start(2);//接受数据UART3_eDMA_TransInit(1);//发送数据//printf("InitialOk.....................");DisableInterrupts;//禁止总中断hw_pit_init(0,20000);//初始化pit0，周期0.001s++++++++++++++12000enable_pit_interrupt(PIT0);//开pit中断enable_irq(87);set_irq_priority(87,6);//PORTAVectoris103.IRQ#is103-16=87enable_irq(88);set_irq_priority(88,5);//PORTBVectoris104.IRQ#is104-16=88enable_irq(91);set_irq_priority(91,4);//PORTEVectoris107.IRQ#is107-16=91enable_irq(2);myDMA_Start(2);//DMA-Channel2每次传输必须要打开EnableInterrupts;Lock_Acce=199;Get_off();//Lock_Threshold=Threshold;call_init();FTM1_CNT=0;FTM2_CNT=0;startline_count=0;while(1){Up_Load();UART_Change();if(flag_run==1)//中断开启程序角度输出{Display();Up_Load();if(flag_init==1){if(Speed_Curr>90){Motor_Cnt++;}elseMotor_Cnt=0;if((Motor_Cnt>20)||AngleSigma>120||AngleSigma<-100){DisableInterrupts;Pwm_Left=0;Pwm_Right=0;PWM_Change(FTM0,0,0);PWM_Change(FTM0,1,0);PWM_Change(FTM0,2,0);PWM_Change(FTM0,3,0);}}36 }//if(flag_run==1)if(flag_upload==1)//DMA串口启动发送{myDMA_Start(1);flag_upload=0;}}//while}//mainvoidUp_Load(){charppp;intpp;ppp=(char)AngleSigma+80;send_pixel[20]=ppp;ppp=(char)Left_Speed+80;send_pixel[21]=ppp;ppp=(char)Right_Speed+80;send_pixel[22]=ppp;pp=(int)Dire_M_Out;//speed_int=((int)get[23]*100+get[24])-5000;send_pixel[23]=(char)((pp+5000)/100);//fi的值send_pixel[24]=(char)(pp+5000-send_pixel[23]*100);}/********************************************************************//********************************************************************///速度控制区/********************************************************************//********************************************************************///功能：100ms调用一次得到速度的计算值并将愈加在电机上//加在电机上分为20个周期加入函数名为Speed_Out()//voidSpeed_Calc(void){signedcharl,r;floatfp,fi;l=Dist_Calc(send_pixel1,DIST_L);r=Dist_Calc(send_pixel1,DIST_R);Speed_Curr=(Right_Speed+Left_Speed)/2;/*****算法一：计算速度--Speed_Curr*****/#defineSpeed_Curr_tog0.0136 Speed_Curr=Speed_Prev*Speed_Curr_tog+Speed_Curr*(1-Speed_Curr_tog);//速度再滤波Speed_Prev=Speed_Curr;if(SPEED_M==1){;}elseif(SPEED_M==2){Speed_Error_Curr=Speed_Curr-SPEED_SET;SPEED_I=SPEED_I_MASK;SPEED_SET=SPEED_SET_MASK;if(black_count==0)//正常运行{if(Speed_Error_Curr>2)//速度过快处理下坡控制{SPEED_P=SPEED_P_MASK+Speed_Error_Curr*1.2;}elseSPEED_P=SPEED_P_MASK;//直道速度控制}}if(abs(r-l)<5)SPEED_SET=SPEED_SET_MASK+2;if(black_count!=0)//检测到黑线之后{//SI1_OFF;//SI1_ONSPEED_P=1;//进入之后默认值5-10-1SPEED_I=0;//Speed_Int/=3;if(black_count<10)//刚进入黑线区45SPEED_P=50;//50//if(black_count>400)//出黑线区135//{//if(Speed_Error_Curr>3)//SPEED_P=SPEED_P_MASK+Speed_Error_Curr*1.2;//出障碍区稳定//else//SPEED_P=40;//}SPEED_SET=black_count*SPEED_SET_MASK/500+10;//5}/*****算法一：计算速度误差--Speed_Error_Curr*****/#defineSpeed_Error_Curr_tog0.01//0.0236 Speed_Error_Curr=Speed_Error_Prev*Speed_Error_Curr_tog+Speed_Error_Curr*(1-Speed_Error_Curr_tog);//速度再滤波Speed_Error_Prev=Speed_Error_Curr;fp=Speed_Error_Curr*SPEED_P;fi=Speed_Error_Curr*SPEED_I*0.01;if(SPEED_M==2){if(black_count==0){if(abs(Speed_Error_Curr)>20)//积分饱和现象fi=0;}elsefi=0;}if((Speed_Error_Curr>=0)&&(Speed_Error_Curr<=2))fi=0;Speed_Int+=fi;if(Speed_Int>SPEED_I_RANGE)Speed_Int=SPEED_I_RANGE;if(Speed_Int<-SPEED_I_RANGE)Speed_Int=-SPEED_I_RANGE;Speed_Out_Old=Speed_Out_New;Speed_Out_New=fp+Speed_Int;//得带输出}voidSpeed_Out(void){floatvalue;if(SPEED_ADJ_ON&&flag_gyro==1){value=Speed_Out_New-Speed_Out_Old;Speed_Per_Out=value*(SPEED_CTRL_n+1)/ENCODER_PERIOD+Speed_Out_Old;//算法一:Speed_M_Out_Curr//////////////////////////////////////////////////////////////////////////////Speed_M_Out_Curr=(int)Speed_Per_Out;//算法二:Speed_M_Out_Curr//////////////////////////////////////////////////////////////////////////////#defineSPEED_Tog0.1//Speed_M_Out_Curr=(int)Speed_Per_Out;//Speed_M_Out_Curr-=Speed_M_Out_Prev*SPEED_Tog;//算法三:Speed_M_Out_Curr////////////////////////////////////////////////////////////////////////////#defineSPEED_Tog0.01Speed_M_Out=(int)Speed_Per_Out;Speed_M_Out_Curr=Speed_M_Out_Prev*SPEED_Tog+Speed_M_Out*(1-SPEED_Tog);36 Speed_M_Out_Prev=Speed_M_Out_Curr;}}/********************************************************************//********************************************************************///角度控制区/********************************************************************//********************************************************************/#defineANG_TOG0.008#defineS2A0.01voidAngle_Out(){floata,b;Angle_P=Angle_P_MASK;Angle_D=Angle_D_MASK;if(black_count!=0){Angle_P+=50;//40Angle_D+=300;}a=AngleSigma*Angle_P+gyro*Angle_D;//正常PID运算b=a*GYRO_PERIOD*0.1;if(ANGLE_ADJ_ON&&flag_gyro==1){Angle_M_Out=b;Angle_M_Out_Curr=b;Angle_M_Out=Angle_M_Out_Curr-Angle_M_Out_Prev*ANG_TOG;Angle_M_Out_Prev=Angle_M_Out;}}/********************************************************************//********************************************************************///方向控制区/********************************************************************//********************************************************************//////////////////////////////////////////////////////////////////////功能：计算方向控制输出量//说明：最终得到Dire_Out_New///////////////////////////////////////////////////////////////////voidDire_Calc(void){Send_Bin(get_pixel+6);//采集并二值化处理Dist_Left=Dist_Calc(send_pixel,DIST_L);Dist_Right=Dist_Calc(send_pixel,DIST_R);36 if(Dist_Left<=0){Dist_Right=64-Dist_Left;}if(Dist_Right<=0){Dist_Left=64-Dist_Right;}if(Dist_Right>80)Dist_Right=80;if(Dist_Right<-80)Dist_Right=-80;if(Dist_Left>80)Dist_Left=80;if(Dist_Left<-80)Dist_Left=-80;Dist_Left_Prev=Dist_Left;Dist_Right_Prev=Dist_Right;if(abs(Dist_Left-Dist_Left_Prev)>10)//5Dist_Left=Dist_Left_Prev;if(abs(Dist_Right-Dist_Right_Prev)>10)//5Dist_Right=Dist_Right_Prev;ZUO=ccd_move_aver_filt(DL_DB,Dist_Left,DIRE_NC);YOU=ccd_move_aver_filt(DR_DB,Dist_Right,DIRE_NC);ZUO_1=ZUO;YOU_1=YOU;DIRE_P=DIRE_P_MASK;DIRE_D=DIRE_D_MASK;ee=(int)(ZUO-YOU);//if(Threshold350)Dire_Per_Out=350;if(Dire_Per_Out<-350)Dire_Per_Out=-350;if(DIRE_ADJ_ON&&flag_gyro==1){////////////////////////////////////////////////////////////////////////#defineDIRE_Tog0.01//Dire_M_Out=(int)Dire_Per_Out;//Dire_M_Out=Dire_M_Out_Prev*DIRE_Tog+Dire_M_Out*(1-DIRE_Tog);////////////////////////////////////////////////////////////////////////#defineDIRE_Tog0.1Dire_M_Out=(int)Dire_Per_Out;//Dire_M_Out-=Dire_M_Out_Prev*DIRE_Tog;//Dire_M_Out_Prev=Dire_M_Out;}}/////////////////////////////////////////////////////////////////////函数功能：将给定的参数PWM1PWM2变换输出到两个电机带有最大输出占空比//PWM1左轮为负值前进PWM1_1//为正值后退PWM1_2//PWM2右轮///////////////////////////////////////////////////////////////////voidMotor_Out(){intMot_L_Q,//1组电机正转Mot_L_H,//2组点击反转Mot_R_Q,Mot_R_H;Pwm_Left=(int)(Angle_M_Out-Speed_M_Out_Curr+Dire_M_Out);Pwm_Right=(int)(Angle_M_Out-Speed_M_Out_Curr-Dire_M_Out);if(Pwm_Left>=800){Pwm_Left=800;}if(Pwm_Left<=-800){Pwm_Left=-800;}36 if(Pwm_Right>=800){Pwm_Right=800;}if(Pwm_Right<=-800){Pwm_Right=-800;}Pwm_Left_Curr=Pwm_Left;Pwm_Right_Curr=Pwm_Right;//算法一：得到Pwm_Left_Curr///////////////////////////////////////////////////////////////#definePWM_Tog0.2//Pwm_Left_Curr-=Pwm_Left_Prev*PWM_Tog;//Pwm_Right_Curr-=Pwm_Right_Prev*PWM_Tog;//算法二：得到Pwm_Left_Curr/////////////////////////////////////////////////////////////#definePWM_Tog0.05Pwm_Left_Curr=PWM_Tog*Pwm_Left_Prev+(1-PWM_Tog)*Pwm_Left;Pwm_Right_Curr=PWM_Tog*Pwm_Right_Prev+(1-PWM_Tog)*Pwm_Right;//算法二：得到Pwm_Left_Curr///////////////////////////////////////////////////////////////#definePWM_Tog0.3//Pwm_Left_Curr=Pwm_Left_Prev*PWM_Tog+Pwm_Left_Curr*(1-PWM_Tog);//Pwm_Right_Curr=Pwm_Right_Prev*PWM_Tog+Pwm_Right_Curr*(1-PWM_Tog);Pwm_Left_Prev=Pwm_Left_Curr;Pwm_Right_Prev=Pwm_Right_Curr;if(Pwm_Left_Curr>0){//左轮前进Mot_L_H=0;Mot_L_Q=(int)Pwm_Left_Curr;Mot_L_Q+=(MOTOR_DEAD_VALUE+30);}else{//左轮后退Mot_L_Q=0;Mot_L_H=0-(int)Pwm_Left_Curr;Mot_L_H+=MOTOR_DEAD_VALUE;}if(Pwm_Right_Curr>0){Mot_R_H=0;Mot_R_Q=(int)Pwm_Right_Curr;Mot_R_Q+=(MOTOR_DEAD_VALUE+30);}else{Mot_R_Q=0;Mot_R_H=0-(int)Pwm_Right_Curr;Mot_R_H+=MOTOR_DEAD_VALUE;}PWM_Change(FTM0,0,Mot_L_Q);PWM_Change(FTM0,1,Mot_L_H);36 PWM_Change(FTM0,2,Mot_R_Q);PWM_Change(FTM0,3,Mot_R_H);}/////////////////////////////////////////////////////////////////////功能：上电调整各个参数值///////////////////////////////////////////////////////////////////voidGet_off(void){floattt;while(flag_run==0){if(flag_button==1){switch(mode){case1:Lock_Acce+=1;break;case2:SPEED_SET_MASK+=2;break;case3:Angle_P_MASK+=20;break;case4:Angle_D_MASK+=50;break;case5:SPEED_P_MASK+=3;break;case6:SPEED_I_MASK+=20;break;case7:DIRE_P_MASK+=5;break;case8:DIRE_D_MASK+=10;break;case9:Threshold_Ratio+=10;break;case10:black_count_mask+=50;if(black_count_mask>500)black_count_mask=0;break;case11:Lock_Threshold+=1;break;}//flag_upload=1;myDMA_Start(1);send_pixel[25]=(char)Angle_P_MASK;send_pixel[26]=(char)(Angle_D_MASK/10);send_pixel[27]=(char)SPEED_P_MASK;send_pixel[28]=(char)SPEED_I_MASK;send_pixel[29]=(char)DIRE_P;send_pixel[30]=(char)DIRE_D;tt=SPEED_SET_MASK;send_pixel[31]=(char)tt;}if(flag_button==2){switch(mode){case1:Lock_Acce-=1;break;case2:SPEED_SET_MASK-=1;break;case3:Angle_P_MASK-=10;break;36 case4:Angle_D_MASK-=30;break;case5:SPEED_P_MASK-=5;break;case6:SPEED_I_MASK-=30;break;case7:DIRE_P_MASK-=2;break;case8:DIRE_D_MASK-=20;break;case9:Threshold_Ratio-=5;break;case10:black_count_mask-=50;if(black_count_mask<100)black_count_mask=100;break;case11:Lock_Threshold-=1;break;}//flag_upload=1;myDMA_Start(1);send_pixel[25]=(char)Angle_P_MASK;send_pixel[26]=(char)(Angle_D_MASK/10);send_pixel[27]=(char)SPEED_P_MASK;send_pixel[28]=(char)SPEED_I_MASK;send_pixel[29]=(char)DIRE_P;send_pixel[30]=(char)DIRE_D;tt=SPEED_SET_MASK;send_pixel[31]=(char)tt;}if(flag_button==3){switch(B3_cnt){case1:mode=1;break;case2:mode=2;break;case3:mode=3;break;case4:mode=4;break;case5:mode=5;break;case6:mode=6;break;case7:mode=7;break;case8:mode=8;break;case9:mode=9;break;case10:mode=10;break;case11:mode=11;break;}}if(flag_button==4){if(mode!=1)flag_run=1;}if(mode==1){GPIO_Ctrl(PTA_BASE_PTR,14,1);GPIO_Ctrl(PTA_BASE_PTR,17,0);}36 else{GPIO_Ctrl(PTA_BASE_PTR,14,0);GPIO_Ctrl(PTA_BASE_PTR,17,1);}flag_button=0;//默认为0Send_Bin(get_pixel+6);Dire_Calc();//Dist_Left=Dist_Calc(send_pixel,DIST_L);//Dist_Right=Dist_Calc(send_pixel,DIST_R);Display_Init();sprintf(temp,"*%2d",Lock_Threshold);//显示当前编码器值LCD_P6x8Str(70,7,temp);UART_Change();if(flag_upload==1){flag_upload=0;}}//while}///////////////////////////////////////////////////////////////////////////功能：系统运行时测试出各种变量的运行状态/////////////////////////////////////////////////////////////////////////voidDisplay(void){intxyz=0;floatttt=0;/////////////////////角度显示////////////////////////////////sprintf(temp,"%-3d",ad_offset[GYRO_CURR]);LCD_P6x8Str(43,2,temp);//陀螺仪偏移量sprintf(temp,"G:%-3d-",gyro_temp);LCD_P6x8Str(0,2,temp);//if(flag_startline!=0)//LCD_P6x8Str(73,7,"(S)");sprintf(temp,"%-3d",ad_offset[GYRO_HOR_CURR]);LCD_P6x8Str(43,3,temp);//陀螺仪偏移量sprintf(temp,"H:%-3d-",gyro_temp_hor);LCD_P6x8Str(0,3,temp);sprintf(temp,"Int:%-4d",(int)gyro_indv);LCD_P6x8Str(20,1,temp);//陀螺仪积分值sprintf(temp,"A:%-3d--",ad_avercal[ACCE_CURR]);36 LCD_P6x8Str(0,4,temp);//角度偏移sprintf(temp,"%-3d",ad_offset[ACCE_CURR]);LCD_P6x8Str(43,4,temp);//加速度当前值-偏移量(上电初固定)sprintf(temp,"%-3d",Angle_P);LCD_P6x8Str(70,2,temp);sprintf(temp,"%-4d",Angle_D);LCD_P6x8Str(103,2,temp);ttt=SPEED_SET_MASK;sprintf(temp,"V=%3d...",(int)ttt);LCD_P6x8Str(80,1,temp);sprintf(temp,"AS=-%6d",(int)(AngleSigma*100));//显示融合后的角度值LCD_P6x8Str(0,5,temp);sprintf(temp,"D:%3d--",(int)Dist_Left);//显示当前编码器值LCD_P6x8Str(0,6,temp);sprintf(temp,"%3d",(int)Dist_Right);//显示当前编码器值LCD_P6x8Str(43,6,temp);xyz=(int)ZUO;sprintf(temp,"DL=%3d",xyz);//显示当前编码器值LCD_P6x8Str(0,7,temp);xyz=(int)YOU;sprintf(temp,"DR=%3d",xyz);//显示当前编码器值LCD_P6x8Str(90,7,temp);sprintf(temp,"%-3d",(int)DIRE_D);LCD_P6x8Str(103,5,temp);sprintf(temp,"%-3d",(int)DIRE_P);//显示当前k2值LCD_P6x8Str(70,5,temp);sprintf(temp,"%-3d",(int)SPEED_I);LCD_P6x8Str(90,4,temp);sprintf(temp,"%-3d",(int)SPEED_P);//显示当前k2值LCD_P6x8Str(70,4,temp);//xyz=Speed_Int;//sprintf(temp,"%-6d",(int)xyz);//LCD_P6x8Str(70,6,temp);sprintf(temp,"Th=%-2d",Threshold);//开显示voidDisplay(void);LCD_P6x8Str(40,7,temp);}36 voidcall_init(){//SPEED_CTRL_n=0;//速度加权//Speed_Per_Out=0;//速度平均输出//Speed_Time=0;//更新时间计数--速度////Speed_Error_Prev=0;//上时刻速度误差//Speed_Error_Curr=0;//当前速度误差//Speed_Int=0;//速度误差积分值////Speed_Curr/=2;//当前速度值//Speed_Prev/=2;//上时刻速度值//Speed_Out_Old=0;//上时刻速度输出//Speed_Out_New=0;//当前速度输出}charUART_Change(void){if((flag_dma_recv==1)&&WIRELESS_ON){if((Rec[0]==0xfd)&&(Rec[1]==0x0f)&&(Rec[2]==0x3C)&&(Rec[3]==0xB8))//开始帧if((Rec[19]==0x00)&&(Rec[20]==0x00))//结束帧{Angle_P_MASK=140+(Rec[4]-140);Angle_D_MASK=1000+(Rec[5]-100)*10;SPEED_P_MASK=100+(Rec[6]-100);SPEED_I_MASK=100+(Rec[7]-100);DIRE_P=15+(Rec[8]-15);DIRE_D=50+(Rec[9]-50);SPEED_SET_MASK=(int)Rec[10];//更新速度值}}flag_dma_recv=0;return1;}#defineCONTROL_PERIOD5#defineUARTJK_PERIOD2//监控状态上传比例*100ms////////////////////////////////////////////////////////////////////////////////voidPit0_isr(void){DisableInterrupts;//关总中断TIM++;//0.5msif(TIM>1000){TIM=0;if(flag_init==1){if(global_time++>=65535)//global_time5sglobal_time=31;36 if((startline_count!=0)&&(global_time>=30))startline_count++;if(global_time<30)startline_count=0;}}if((PIT_TFLG(0)&PIT_TFLG_TIF_MASK)!=0){PIT_TFLG(0)|=PIT_TFLG_TIF_MASK;//清标志if(flag_run==1){if(flag_init==0){startline_count=0;if(flag_gyro==0){//上电采集陀螺仪电压gyro_off_add+=ad_ave(ADC0,SE17,ADC_10bit,6);if(gyro_time++>2048){flag_gyro=1;Lock_Gyro=gyro_off_add>>11;}}if(flag_gyro==1)TIME_INIT++;if(TIME_INITTIME_UPRIGHT)&&(TIME_INIT<=TIME_UPRIGHT+TIME_ACCE)){SPEED_P=5;SPEED_I=80;//300,40036 SPEED_SET=SPEED_SET_MASK-(float)SPEED_SET_MASK*(float)((TIME_INIT-TIME_UPRIGHT)/TIME_ACCE);}//方向打开if(TIME_INIT>(TIME_UPRIGHT+TIME_ACCE)){SPEED_M=2;Speed_Int=0;SPEED_SET=SPEED_SET_MASK;if(DIRE_ADJ_ON_MASK)DIRE_ADJ_ON=1;Angle_P_MASK+=10;Angle_D_MASK+=100;//SPEED_I_RANGE=200;flag_init=1;}}}if(flag_run==1){SPEED_CTRL_n++;//速度细化输出加权值Speed_Out();DIRE_CTRL_n++;//方向细化输出次数计数加权值Dire_Out();//方向平均输出开关}if(Control_Count>=CONTROL_PERIOD){//任务0Control_Count=0;if(flag_run==1){Get_Speed();if(flag_black==1){if(black_count++>black_count_mask){//200flag_black=0;black_count=0;}}}if(flag_run==0){L_QD=(int)FTM2_CNT;if(L_QD>32767){L_QD-=65536;}//初始化检测计数脉冲R_QD=(int)FTM1_CNT;if(R_QD>32767){R_QD-=65536;}//初始化检测技术脉冲}}elseif(Control_Count==1){//任务一Get_Ad();}elseif(Control_Count==2){//任务二if((flag_run==1)&&(flag_gyro==1)){36 Roll_Angle();//积分转换成角度Angle_Out();//直立控制输出量计算Motor_Out();}}elseif(Control_Count==3){//任务三if(flag_run==1){Speed_Time++;if((Speed_Time>=20)&&(SPEED_ADJ_ON)){//50msSpeed_Calc();Speed_Time=0;SPEED_CTRL_n=0;if(count++>UARTJK_PERIOD){flag_upload=1;count=0;}//200ms串口上传数据if(count_S_i++>120){//SPEED_I_RANGE=2000;//3500}}}}elseif(Control_Count==4){//任务四5msif(flag_run==1){if((DIRE_ADJ_ON)){Dire_Time++;if(Dire_Time>=2){//5msDire_Calc();Dire_Time=0;DIRE_CTRL_n=0;}}}}Control_Count++;//虽然每次运行任务为1MS但是此变量等于为每个任务进行5MS的分频控制}PIT_TFLG(0)|=PIT_TFLG_TIF_MASK;//清标志EnableInterrupts;//开总中断}36'