基于dsp的全向运动控制系统软件设计 24页

基于DSP的全向运动控制系统软件设计基于DSP的全向运动控制系统软件设计[摘要]本文基于DSPC2000系列TMS320LF2407A核心控制芯片，以CCStudioV3.3软件为开发平台，在了解RoboCup中型组足球机器人和其他全向机器人的基础上，主要完成了全向运动控制系统的软件设计。通过对全向运动控制系统的研究，建立了三轴全向运动学数学模型，并对整体平移运动、原地旋转运动、边平移边旋转三种运动方式进行分析和数学建模；利用上位机，经无线模块发送运动方式和各种运行参数，控制运动系统实现各种运动模式；经全向运动控制系统软件的编写和与相关硬件联机调试，系统实现了预订的全向运动形式。通过试验验证和结果分析，各种运动状态的准确性已达到性能的基本要求。[关键词]DSP；全向运动控制；数学建模；MATLAB仿真；串行通信;软件设计SoftwareDesignofOmni-directionalMotionControlSystemBasedonDSPAbstract:ThisarticleisbasedonTheTMS320LF2407AintheseriesofDSPC2000asthecontrolcorechipandthesoftwareofCCStudioV3.3asthedevelopmentplatform.Itisbasisontheunderstandingofmedium-sizedgroupaboutsoccerrobotinRobocupandotheromnidirectionalrobot,mainlyaccomplishthesoftwaredesignofaOmni-directionalMotionControlSystem.Throughtheresearchtothemotioncontrolsystem,motioncontrolsystemwasestablishedtoallthreeaxiskinematicsmathematicmodel,buildthemathematicalmodelingandanalysisthreekindofmovement,whichincludethewholetranslationmovement,spinaroundmovementandedgetranslationandrotating.UsingthePC,Thewirelessmoduletosendthemovementandavarietyofoperatingparameters,controlmotionsystemtoachieveavarietyofmotionmodes;Bythewholemotioncontrolsystemsoftwaretowriteandonlinecalibrationwithrelevanthardware,thesystemrealizedtheomnidirectionalmovementweexpected.Throughtheresultsanalysisandexperimentalverification,theaccuracyhavealreadymeetrequirementsofvarioussportsstate.KeyWords:DSP;omnidirectionalmotioncontrol;mathematicalmodeling;MATLABsimulation;serialcommunication；softwaredesignIII\n基于DSP的全向运动控制系统软件设计目录1引言11.1全向运动控制系统发展现状11.2本课题的研究意义及前景21.3论文组织结构32全向运动控制系统分析32.1全向运动控制系统运动学模型建立32.2不同运动方式的运动特性52.2.1平移运动52.2.2原地旋转运动72.3.3边平移边旋转运动83基于DSP的硬件系统简介83.1控制芯片选择83.2硬件系统结构图93.3硬件系统基本模块104系统运动控制的MATLAB仿真134.1电机PID控制134.2转速检测144.3MATLAB仿真155系统软件设计165.1软件开发平台及仿真器165.2运动控制软件设计185.2.1主程序185.2.2三种基本运动状态子程序195.2.3电机控制子程序205.2.4无线发送子程序205.3上位机软件及通讯协议216实验验证及结果分析226.1试验场地236.2各项性能测试246.2.1速度PID测试24III\n基于DSP的全向运动控制系统软件设计6.2.2平移运动测试246.2.3原地旋转测试256.2.4平移+旋转运动测试266.3影响因素分析26结束语28参考文献29附录30致谢42III\n基于DSP的全向运动控制系统软件设计1引言随着机器人技术的日新月异，机器人应用领域也已从工业走向普通生活。机器人技术已经在人们的生活中越来越发挥着重要的作用。作为机器人中的全向运动机器人，有着其特殊的运动形式，即可以不改变姿势向任何方向运动，找到最佳位置。同时也可以边往某个方向运动，边调整自身的姿势达到最佳运动效果，可以在平面内完美的往任何方向做三自由度运动。全向运动机器人其高灵活度，势必会在机器人领域发挥不可代替的作用。在全向运动机器人中现阶段比较常见的是三轮结构和四轮结构（如图1所示）。通过控制各个轮子间相互协调运动达到全方位的运动效果，包括全方位平移、全方位边平移边旋转和原地旋转运动形式。要完整文章毕业论文的+扣扣：（149-908-83-24）去掉中间的横线图1三轮和四轮的结构模型2全向运动控制系统分析2.1全向运动控制系统运动学模型建立在引言中介绍了全向运动控制的几种结构，本设计选用三轮结构构建全向运动系统[3][4]。其结构图和实物付如图4所示。21\n基于DSP的全向运动控制系统软件设计(a)结构图(b)实物图图4全向结构图和实物图由图5中(b)所示，V为机器人系统整体速度，世界坐标X-Y坐标系中X轴与整体速度V的逆时针夹角为β，在世界坐标系中将整体速度V分解成沿X轴和Y轴的速度分别为和。2.2不同运动方式的运动特性全向运动的运动形式大体可分为平移运动、原地旋转运动、边平移边旋转运动三种运动形式。三种运动形式有着不同的运动特性，以下将分三小节对三种运动形式进行单独分析。2.2.1平移运动平移是基本的也是比较常用的运动形式。由于是平移不考虑旋转运动，我们将模型进行简化，其简化后运动学模型图如图6所示。(a)平移模型(b)系统整体平移模型21\n基于DSP的全向运动控制系统软件设计图6简化后运动运动学模型单独考虑平移运动故将式(3)进行简化为式(4)：(4)假设小车整体平移矢量速度为V，与X轴成γ角。所以我们可以分解为和，其分解公式为式(5)。(5)由式(4)和式(5)我们可以得到三个驱动轮的速度，见式(6)。(6)驱动轮速度、、的速度范围都是。的值则由电机性能决定。通过式(6)我们可以反向推导出平移速度V的速度范围。如图7，其中圆是以驱动轮最大速度大小为半径的圆，红色速度V与X轴的角度为γ，随着γ由0°~360°变化V的轨迹为红色正六边形。其中最大速度为。21\n基于DSP的全向运动控制系统软件设计图7平移最大速度模型3基于DSP的硬件系统简介3.1控制芯片选择作为运动控制系统的核心控制芯片，根据以往在实验室的经验，其核心控制芯片当属DSPC2000系类。C2000系类对于运动控制有着诸多的优点，本设计采用C2000系类TMS320LF2407A的控制板。TMS320LF2407A主要特点如下：3.2硬件系统结构图通过第2章关于运动学模型的建立，为使得系统能够按照建立的数学模型进行运动，除了3.1节的DSP主控制芯片外还需要其它的硬件的支持，本课题设计的硬件系统结构图如图9所示。其主要的工作方式为：PC上位机选择相应的运动方式，经过RS232协议连接无线模块，由无线模块发送数据。DSP下位机的无线模块接收上位机发送的数据，经RS232通信协议发送给DSP。DSP接收到数据进行相应的处理，分析出要进行的运动形式及各种参数，同时利用I2C协议采集指南针的数据，将这些信息通过第2章的全向运动学模型分析的各种公式转换成各个电机的速度值。为使得电机能够稳、准、快的执行，本设计采用经典PID控制，以增量式编码器采集的值为反馈。在进行的过程中液晶模块实时显示运动方式和参数及指南针的值，同时将编码器的值通过无线模块传送给PC上位机。DSP下位机将按照程序设定执行上位机设定的运动方式，只有上位机重新发送新的运动方式才会改变。3.3硬件系统基本模块根据图9硬件系统结构图所示，本节将分析几个重要的模块。分为电源模块、无线通信模块、指南针模块、液晶显示模块，DSP下位机模块、电机及驱动模块和全向轮。其中DSP下位机3.1节已经介绍，在此不再叙述。要完整文章毕业论文的+扣扣：（149-908-83-24）去掉中间的横线4系统运动控制的MATLAB仿真本设计的全向运动控制系统使用三轴全向控制，即控制3个电机相互协调运动来实现全向轮动。电机的控制对整个系统是至关重要的。本章主要分析系统的PID控制和MATLAB仿真。4.1电机PID控制21\n基于DSP的全向运动控制系统软件设计电机是运动系统的执行机构，不管DSP计算的多么正确，但是电机执行达不到要求，同样系统是无法实现全向运动的。为使得电机能够稳、准、快的执行，而PID是根据偏差的比例、积分、微分的线性组合进行反馈控制，是迄今为止工业中应用最为广泛的一种控制方法。所以本设计采用PID分别对三个电机进行闭环控制。4.2转速检测本设计选用增量式PID即4.1节中所阐述的PID。对于PID控制反馈必不可少，在本设计使用的是Faulhaber带双路编码器减速电机，即增量式光电编码器来做为反馈环节，其工作原理如图14。光源经过码盘和检测光栅照射的信号通过光电检测器件检测，检测出的正弦值经转换电路转换成矩形波，最后出来A相B相Z相。A相B相相差90度，通过软件能够很好的得到此时电机的速度和方向。对AB相的采集是电机闭环控制的反馈量，关乎电机PID控制是否能达到稳准快的要求。本设计使用输入捕捉CAP功能对脉冲进行采集，将采集量进行相应处理即得到PID控制的反馈。4.3MATLAB仿真在三轴控制系统中，PID是应用最广泛的控制算法。针对电机模型，这里设定电机的模型为，针对此电机模型进行MATLAB仿真，观察PID控制算法的控制效果[10][11]。控制结果比较曲线如图15所示。绿色曲线为期望值，蓝色曲线为PID控制跟踪曲线，红色曲线为误差曲线，实际速度可以通过编码器反馈。可以看出PID控制方式的响应速度、跟踪精度可以达到系统要求。要完整文章毕业论文的+扣扣：（149-908-83-24）去掉中间的横线5系统软件设计根据前几章模型建立分析和硬件系统构建，本章主要介绍全向运动控制系统程序编写。前两节介绍了下位机全向运动系统使用的程序编写平台软件和整个系统流程图及各部分子程序的流程图。最后一节介绍上位机部分，编写上位机软件部分和上位机下位机使用的通讯协议。5.1软件开发平台及仿真器本设计的核心控制芯片为TMS320LF2407A，其软件开发平台使用官方工具CCSv3.3，仿真器选用XDS510-USB2.0。安装CCSV3.3后生成两个部分，即CCStudioV3.3和SetupCCStudiov3.3。其中利用SetupCCStudiov3.321\n基于DSP的全向运动控制系统软件设计对开发工具参考硬件进行相应配置，利用CCStudioV3.3建立工程编写软件,其开发周期示意图为图18所示[12]。图19即为CCStudioV3.3开发环境。5.2运动控制软件设计由于本课题是基于DSP全向运动控制系统软件设计，侧重控制全向运动，故本设计将检测部分做了精简，只需要采集指南针和编码脉冲。其全向运动方式经过上位机无线传输指令，然后DSP接收指令进行处理最后控制电机协调运动，根据指南针和编码器进行闭环控制，以达到最佳效果。6实验验证及结果分析为验证本设计的各种全向运动是否准确，本章将对各种全向运动形式进行试验验证。设计试验场地，记录测试试验数据，对结果进行分析。6.1试验场地由于本设计使用的驱动轮为尼龙材质。驱动轮与地面接触部分硬度较大，如果在普通地板上驱动轮将得不到足够大的摩擦力，有严重的“打滑”现象。为提供足够摩擦力结合实验室条件，选择飞思卡尔赛道使用的KT板，做成120CM*100CM的试验场地，并在场地上标上边距为20cm的方格，同时为验证转角正确性标上了相隔45°的虚线，结构图见图26。6.2各项性能测试需要试验验证测试的有：PID验证、平移验证、原地旋转验证、边平移边旋转运动。设计制作试验装置，分别对各种性能测试。6.3影响因素分析影响系统性能的因素大部分来源于外部，大致分为以下几种情况：（1）电池电量不足，导致系统电机启动或大的变化时电流不足。导致系统不能很好的按照设定的形式运动。(2）外部磁场干扰，数字指南针能够很灵敏的感应磁场，所以外部稍强点的磁场就能干扰系统，一方面是金属铁，其很容易磁化，导致变成弱磁铁。在室内钢筋结构的环境下各个位置受到的磁干扰不同，经测试最大可影响角度达30°角，严重影响了系统性能。另一方面是电机，由于电机带有磁性较强磁铁，产生磁场的磁场也对指南针有一定干扰。21\n基于DSP的全向运动控制系统软件设计去除干扰的方法只能经过硬磁补偿来减少很少部分干扰，大多数干扰无法去除。(3）驱动轮干扰，本设计使用的是瑞典全向轮，轮子外侧有两排相互交错的侧轮，使得驱动轮滚动过程中，造成上下震动。同时由于驱动轮滚动会造成电机力矩发生变化，L=12或14.5cm，及内外两个侧滑轮着地时会造成力矩2.5cm的变化，会使得系统转动发生误差。要完整文章毕业论文的+扣扣：（149-908-83-24）去掉中间的横线结束语全向运动控系统在全向机器人上的应用可实现机器人向任意方向做直线运动而不需要事先作旋转运动，并且在以直线运动到达目标点的过程中同时可以做自身旋转运动，以调整机器人的姿态，从而达到终态所需的姿态角。其特殊的移动方式使得全向机器人越来越得到重视和应用，其未来的实用领域和前景将更加宽广。本设计完成的主要工作为以下几点：(1)对目前的几种运动结构进行了研究分析，设计了互成120度的三轴运动结构。(2)对三轴运动控制结构进行了数学建模，进行了运动学分析。对全向运动的运动的几种运动形式进行了详细的分析计算。(3)运用TMS320LF2407A芯片为控制核心，使用了其模块UART（SCI）、IIC（模拟）、PWM、I/O、CAP、外部扩展RAM。同时围绕核心的外部器件进行程序的编写调试，已达到全向控制的目的。(4)使用了VC6.0的MSComm控件编写上位机软件，设计上位机界面和功能，编写通信协议。使得系统能够经过选择相关运动形式，修改上位机的相关参数来实现运动系统的全向运动。同时能够在上位机显示系统现在运动的状态和三个电机速度值。并实现了通过游戏手柄来遥控系统的各种运动状态。(5)对多电机实现了数字增量式PID控制，三个电机各自PID控制且互不影响，并通过MATLAB进行了仿真。使得计算出的速度能够很好的执行，提高了系统的准确性。(6)实现了全向运动控制系统的所有运动形式，且经过试验测试验证了其准确性。参考文献[1]徐旭,李实,叶榛,孙增圻.ASurvey:RoboCupandtheResearchProceedingsofthe3rdWorldCongressonIntelligentControlandAutomation[M],2008[2]赵冬斌,易建强,邓旭玥.全方位移动机器人结构和运动分析[J].机器人.2003(25)[3]海丹.全向移动平台的设计与控制[D].长沙:国防科技大学.200921\n基于DSP的全向运动控制系统软件设计[4]龚建伟.VisualC++/TurboC串口通信编程实践.北京.电子工业出版社，2004[5]RaulRojas:OmnidirectionalControl,FreieUniversityBerlin,TechnicalReportB-10-03,FU.Bedin.June2003．[6]刘和平.DSP原理及电机控制应用—基于TMS320LF240X系列[M].北京:北京航空航天大学出版社,2008.[7]王茂飞,程昱.TMS320C2000DSP技术与应用开发[M].北京:北京清华大学出版社,2007.[8]TexasInstruments.TMS320LF/LC240xADSPControllersReferenceGuide-SystemandPeripheralsSPRU357B[M].December,2001.[9]陶永华，尹怡欣，葛芦生.新型PID控制及其应用[M].北京.机械工业出版社.1998[10]薛定宇.控制系统计算机辅助设计-MATLAB语言与应用（第2版）[M].北京:清华大学出版社,2006.[11]范海艇.基于NiosⅡ的中型足球机器人底层控制系统的研究与实现[D].上海,上海大学.[12]姜艳波.数字信号处理器-DSP应用100例[M]，北京：化学工业出版社，2007[13]海丹.全向移动平台的设计与控制[D].长沙:国防科技大学，2006[14]张宏林.精通VisualC++串口通信技术与工程实践(第3版).北京.人民邮电出版社，2008[15]龚建伟.VisualC++/TurboC串口通信编程实践（第二版）[M],北京：电子工业出版社，2007.9附录基于DSP的全新运动控制系统软件清单如下:21\n基于DSP的全向运动控制系统软件设计其中自己建的有GB161.H（存储液晶字模数据），24C02.C（操作指南针），design.C（运动控制策略），ILI9163.C（液晶显示），main.c（主程序）。由于程序比较大，在此只附录上main.c和design.c两个文件。Main.c：/******************************************************************************//*FileName:main.c*//*Autor:LiHai-qing*//*Date:04/01/2011*//*Description:Thisfileprovidesmainfunction,provideinitialization*//*串口发送与中断接收程序,波特率设置为9600*//*IO口占用：：PE1--PE6（PWM）PA3--PA6和PB0-PB7和PE7液晶屏使用*//*PF0=CAP5,PF1=cap6,PA3=cap1三电机测速。PE2=PWM8PE4=PWM10，PE6=PWM12三电机*//*PC0-1PF2-3PF4-5电机正反转*//******************************************************************************/#include"global.c"#include"math.h"voidSystemInit();voidTimer1Init();voidILI9163_init();voidKickDog();voidPWM_Init();voidIOinit();voidTimer4Init();voidTimer2Init();/****************************************************/21\n基于DSP的全向运动控制系统软件设计/*主程序*//****************************************************/main(){SystemInit();//系统初始化IOinit();//IO口初始化SCI_Init();//SCI初始化PWM_Init();//PWM初始化ILI9163_init();//液晶初始化Timer1Init();/*定时器1初始化*/Timer4Init();/*定时器4初始化*/Timer2Init();/*定时器2初始化*/asm("CLRCINTM");display(0xff07,0xffe0);LCD_PutString(40,03,"李海清",0x001f,0x07e0);//液晶界面显示信息LCD_PutString(16,23,"运动控制系统",0x001f,0x07e0);//LCD_PutString(0,143,"指南针:",0x001f,0x07e0);//while(1){SciRed();//判断并读取SCI数据speedPV();//速度函数}}/************************************************//*系统初始化*//************************************************/voidSystemInit(){asm("SETCINTM");/*关闭总中断*/asm("CLRCSXM");/*禁止符号位扩展*/asm("CLRCCNF");/*B0块映射为on-chipDARAM*/asm("CLRCOVM");/*累加器结果正常溢出*/SCSR1=0x834E;/*系统时钟CLKOUT=20*2=40M*//*打开ADC,EVA,EVB,CAN和SCI的时钟*/WDCR=0x006F;/*禁止看门狗,看门狗时钟64分频*/KickDog();/*初始化看门狗*/IFR=0xFFFF;/*清除中断标志*/IMR=0x000B;/*打开中断1,2*/}/************************************************//*IO口初始化*//************************************************/voidIOinit(){MCRC=MCRC&0x0354;//PE0-PE7，PF0-PF5有效0000MCRA=MCRA&0x0008;//PA0-PA7，PB0-PB7有效0208MCRB=MCRB&0xFF00;PADATDIR=0xF708;//PA口初始化值为0PBDATDIR=0xFF00;//PB口初始化值为021\n基于DSP的全向运动控制系统软件设计PEDATDIR=0xFF7F;//PE口初始化值为0PFDATDIR=0xFC33;//PF0-5为1PF6输入PCDATDIR=0xFFFF;//pc输出pc7-pc001010101}/************************************************//*PWM初始化*//************************************************/voidPWM_Init(){MCRC=MCRC|0x0054;//PE1-PE6为PWM口7EEVBIFRA=0xFFFF;//清除中断标志ACTRB=0x0666;//PWM12,10,8低有效,PWM11,9,7高有效//DBTCONA=0x0530;//使能死区定时器1,分频40M/16=2.5M,死区时间5*0.4us=2usT3PR=2500;//定时器3周期值,定时0.4us*2500=1ms//***2500CMPR4=0;//比较值CMPR5=0;//比较值CMPR6=0;//比较值COMCONB=0xA600;//比较控制寄存器T3CNT=0;//定时器3计数器清零EVBIMRA=0x0080;//定时器3周期中断使能T3CON=0x144E;//增模式,TPS系数40M/16=2.5M,T3使能,}voidKickDog()/*踢除看门狗*/{WDKEY=0x5555;WDKEY=0xAAAA;}design.c：/******************************************************************************//*FileName:design.c*//*Autor:LiHai-Qing*//*Date:03/10/2011*//*Description:ThisfileprovidesMotioncontrolstrategy*//******************************************************************************/#include"GLOBAL.C"#include"math.h"#definespeed1CMPR4//宏定义speed1电机1速度#definespeed2CMPR5//宏定义speed1电机2速度#definespeed3CMPR6//宏定义speed1电机2速度#definePVtime3//扫描时间，每个轮子检测PVtime个ms#defineKp0.8//宏定义Kp#defineKi0.2//宏定义Ki#defineKd0.2//宏定义Kd#definePI3.14159//宏定义PI21\n基于DSP的全向运动控制系统软件设计unsignedintflog=0,flogF=0,flogH=0,flogX=0,flogPID=0;//各种运动状态标志位unsignedintflogPV=0,pvlcd=0,flogPV1=0,flogPV2=0,flogPV3=0;unsignedintSCI_RXDATA,RXDINT[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};intsum11=0,sum22=0,sum33=0;intsum11_0=0,sum22_0=0,sum33_0=0;intSCIn=0,i=0,SCISP1,SCISP2,SCISP3;intspeed1_1=0,speed2_1=0,speed3_1=0,speed1_0=0,speed2_0=0,speed3_0=0;inttt5=0,tt6=0,tt1=0,t0=0,tt5_3[3]={0,0,0},tt6_3[3]={0,0,0},tt1_3[3]={0,0,0};unsignedinttt55=0,tt66=0,tt11=0;//电机编码器值tt55tt66tt11分别电机1-3的离散值unsignedintscitt9[9]={0,0,0,0,0,0,0,0,0};unsignedintPVlb=0,CY_26_read=0,CY_26_Angle=0;unsignedintSCIHspeed=0,SCIHangle=0,SCIHag_SN=0;unsignedintSCIXspeed=0,SCIXangle=0;unsignedintflogHStop=0,flogXSN=0;floatflogangle=0.0,angle11=0.0,angle22=0.0,angle33=0.0,angle=0.0;charSCI_RXDATA1[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};/******************SCI初始化**********************/voidSCI_Init(){MCRA=MCRA|0x0003;//IOPA0,IOPA1为串口SCICCR=0x0007;//空闲多处理器模式，8位数据位,1位停止位，无奇偶校验位SCICTL1=0x0003;//接收，发送，内部时钟使能，SLEEP=1SCICTL2=0x0002;//接收中断使能SCIPRI=0x0000;//高中断优先级SCIHBAUD=0x0002;//40M,波特率设置为9600SCILBAUD=0x0008;SCICTL1=0x0023;//串口初始化完成}/*********************定时器1初始化************************/voidTimer1Init(){T1PR=2500;//定时器1初值,定时0.4us*2500=1msT1CNT=0;GPTCONA=0x0000;EVAIMRA=EVAIMRA|0x0080;//定时器1周期中断使能EVAIFRA=EVAIFRA|0xFFFF;//清除中断标志T1CON=0x144E;//增模式,TPS系数40M/16=2.5M,T1使能}/*********************定时器1-2初始化************************/voidTimer2Init(){T2PR=2500;T2CNT=0;WSGR=0x0000;CAPCONA=0x2240;CAPFIFOA=0x0100;21\n基于DSP的全向运动控制系统软件设计EVAIMRC=EVBIMRC|0x0001;EVAIFRC=EVBIFRC|0xffff;T2CON=0x1440;//1440}/***************定时器4初始化输入捕捉********************/voidTimer4Init(){T4PR=2500;T4CNT=0;WSGR=0x0000;CAPCONB=0x3014;CAPFIFOB=0x1500;EVBIMRC=EVBIMRC|0x0006;EVBIFRC=EVBIFRC|0xffff;T4CON=0x1440;//1440}/******************SCI发送**********************/voidSCI_Send(unsigneddata)//发送0x80-0x87,共8个字节数据{SCITXBUF=data;while((SCICTL2&0x0080)==0);}/******************SCI接收分解运动数据**********************/voidSCIF()//执行速度分解程序，分别控制三个电机的速度{unsignedinti=1;for(i=1;i<14;i++){RXDINT[i]=SCI_RXDATA1[i]-48;//接收转换}if(flogF==0||flogX==1||flogH==1){LCD_clear(0,63,128,64,0xff07,0xffe0);flogX=0;flogH=0;LCD_PutString(16,63,"分解运动",0x001f,0x07e0);LCD_PutString(0,83,"电机1:",0x001f,0x07e0);LCD_PutString(0,103,"电机2:",0x001f,0x07e0);LCD_PutString(0,123,"电机3:",0x001f,0x07e0);}SCISP1=RXDINT[1]*1000+RXDINT[2]*100+RXDINT[3]*10+RXDINT[4];SCISP2=RXDINT[5]*1000+RXDINT[6]*100+RXDINT[7]*10+RXDINT[8];SCISP3=RXDINT[9]*1000+RXDINT[10]*100+RXDINT[11]*10+RXDINT[12];LCD_Num(48,83,SCISP1,0x001f,0x07e0);LCD_Num(48,103,SCISP2,0x001f,0x07e0);LCD_Num(48,123,SCISP3,0x001f,0x07e0);//电机1PC0-1电机2pf2-3电机3pf4-5PCDATDIR=0xFFFF;PFDATDIR=PFDATDIR|0xFCFC;if(RXDINT[1]==1)PCDATDIR=PCDATDIR&0xFFFE;elsePCDATDIR=PCDATDIR&0xFFFD;if(RXDINT[5]==1)PFDATDIR=PFDATDIR&0xFCFB;elsePFDATDIR=PFDATDIR&0xFCF7;21\n基于DSP的全向运动控制系统软件设计if(RXDINT[9]==1)PFDATDIR=PFDATDIR&0xFCEF;elsePFDATDIR=PFDATDIR&0xFCDF;sum11=SCISP1%1000;sum22=SCISP2%1000;sum33=SCISP3%1000;if(sum11==0){PCDATDIR=PCDATDIR|0xFFFF;flogPV1=0;}elseflogPV1=1;if(sum22==0){PFDATDIR=PFDATDIR|0xFC0C;flogPV2=0;}elseflogPV2=1;if(sum33==0){PFDATDIR=PFDATDIR|0xFC30;flogPV3=0;}elseflogPV3=1;flogF=1;//分解运动标志位，控制显示部分清屏，显示相应运动方式和数据}/******************SCI接收合成平移运动数据**********************/voidSCIH()//执行速度合成平移运动，控制整体速度和角度两个值{floatCYanglePI=0.0,CYangleSD=0.0;intCYspeed=0;intflogAnglePI=0,flogaa=0;intHSDangle=0;intHSD_SCIangle=0,absangle=0;HSDangle=flogangle*180/PI+180;if(HSDangle>=360)HSDangle=HSDangle-360;HSD_SCIangle=HSDangle-CY_26_Angle;//设定角度与实际角度的差值absangle=abs(HSD_SCIangle);//设定与实际差距的绝对值if(RXDINT[10]==1){if(absangle>180){if(HSD_SCIangle>-358&&HSD_SCIangle<-180)CYspeed=15+(360-absangle)/5;elseif(HSD_SCIangle<358&&HSD_SCIangle>180)CYspeed=-15-(360-absangle)/5;elseCYspeed=0;}if(absangle==180)Cyspeed=15;else;//防止抖动if(absangle<180){if(HSD_SCIangle>1&&HSD_SCIangle<180)CYspeed=15+absangle/5;elseif(HSD_SCIangle<-1&&HSD_SCIangle>-180)CYspeed=-15-absangle/5;elseCYspeed=0;}}elseCYspeed=0;CYangleSD=PI/2-flogangle;if(flogangle<=PI)flogAnglePI=(int)(flogangle*180/PI+180);elseflogAnglePI=(int)(flogangle*180/PI-180);flogaa=CY_26_Angle-flogAnglePI;CYanglePI=PI-CY_26_Angle*PI/180;sum11_0=(int)SCIHspeed*sin(PI/6-CYanglePI+CYangleSD)+CYspeed;sum22_0=-(int)SCIHspeed*cos(CYangleSD-CYanglePI)+CYspeed;21\n基于DSP的全向运动控制系统软件设计sum33_0=(int)SCIHspeed*sin(PI/6+CYanglePI-CYangleSD)+CYspeed;PCDATDIR=0xFFFF;PFDATDIR=PFDATDIR|0xFCFC;if(sum11_0>=0)PCDATDIR=PCDATDIR&0xFFFE;elsePCDATDIR=PCDATDIR&0xFFFD;if(sum22_0>=0)PFDATDIR=PFDATDIR&0xFCFB;elsePFDATDIR=PFDATDIR&0xFCF7;if(sum33_0>=0)PFDATDIR=PFDATDIR&0xFCEF;elsePFDATDIR=PFDATDIR&0xFCDF;sum11=abs(sum11_0);sum22=abs(sum22_0);sum33=abs(sum33_0);if(flogHStop==1){sum11=0;sum22=0;sum33=0;}else;if(sum11==0){PCDATDIR=PCDATDIR|0xFFFF;flogPV1=0;}elseflogPV1=1;if(sum22==0){PFDATDIR=PFDATDIR|0xFC0C;flogPV2=0;}elseflogPV2=1;if(sum33==0){PFDATDIR=PFDATDIR|0xFC30;flogPV3=0;}elseflogPV3=1;if(flog==1){LCD_Putnum(48,83,SCIHspeed,0x001f,0x07e0);LCD_Num(48,103,SCIHag_SN,0x001f,0x07e0);}flogH=1;//合成平移运动标志位，控制显示部分清屏，显示相应运动方式和数据}/******************SCI接收旋转运动数据**********************/voidSCIX()//执行旋转运动，控制旋转{intSDangle=0;intSD_SCIangle=0,Xspeed=0,absangle=0;SDangle=SCIXangle+180;if(SDangle>=360)SDangle=SDangle-360;SD_SCIangle=SDangle-CY_26_Angle;absangle=abs(SD_SCIangle);if(absangle>180){if(SD_SCIangle>-358&&SD_SCIangle<-180)Xspeed=15+(360-absangle)/2;elseif(SD_SCIangle<358&&SD_SCIangle>180)Xspeed=-15-(360-absangle)/2;elseXspeed=0;}if(absangle==180)Xspeed=15;else;//防止抖动if(absangle<180){if(SD_SCIangle>1&&SD_SCIangle<180)Xspeed=15+absangle/2;elseif(SD_SCIangle<-1&&SD_SCIangle>-180)Xspeed=-15-absangle/2;elseXspeed=0;}sum11_0=Xspeed;sum22_0=Xspeed;sum33_0=Xspeed;PCDATDIR=0xFFFF;PFDATDIR=PFDATDIR|0xFCFC;if(sum11_0>=0)PCDATDIR=PCDATDIR&0xFFFE;elsePCDATDIR=PCDATDIR&0xFFFD;if(sum22_0>=0)PFDATDIR=PFDATDIR&0xFCFB;elsePFDATDIR=PFDATDIR&0xFCF7;if(sum33_0>=0)PFDATDIR=PFDATDIR&0xFCEF;elsePFDATDIR=PFDATDIR&0xFCDF;21\n基于DSP的全向运动控制系统软件设计sum11=abs(sum11_0);sum22=abs(sum22_0);sum33=abs(sum33_0);if(sum11==0){PCDATDIR=PCDATDIR|0xFFFF;flogPV1=0;}elseflogPV1=1;if(sum22==0){PFDATDIR=PFDATDIR|0xFC0C;flogPV2=0;}elseflogPV2=1;if(sum33==0){PFDATDIR=PFDATDIR|0xFC30;flogPV3=0;}elseflogPV3=1;if(flog==1){LCD_Putnum(48,83,SCIHspeed,0x001f,0x07e0);LCD_Num(48,103,SCIHag_SN,0x001f,0x07e0);}flogX=1;//旋转运动标志位，控制显示部分清屏，显示相应运动方式和数据}/***********************************速度PID**************************************/intee1=0,ee2=0,ee3=0,ee1_2=0,ee1_1=0,ee2_2=0,ee2_1=0,ee3_2=0,ee3_1=0;voidPIDPV(){ee1_2=ee1_1;ee1_1=ee1;ee2_2=ee2_1;ee2_1=ee2;ee3_2=ee3_1;ee3_1=ee3;ee1=sum11-tt55;ee2=sum22-tt66;ee3=sum33-tt11;if(flogPID==1){speed1_1=0;speed2_1=0;speed3_1=0;flogPID=0;}//每改变运动形式速度变0speed1_1=speed1_1+Kp*(ee1-ee1_1)+Ki*ee1+Kd*(ee1-2*ee1_1+ee1_2);//KpKiKdspeed2_1=speed2_1+Kp*(ee2-ee2_1)+Ki*ee2+Kd*(ee2-2*ee2_1+ee2_2);speed3_1=speed3_1+Kp*(ee3-ee3_1)+Ki*ee3+Kd*(ee3-2*ee3_1+ee3_2);if(speed1_1<0)speed1_1=0;if(speed2_1<0)speed2_1=0;if(speed3_1<0)speed3_1=0;if(speed1_1>500)speed1_1=500;if(speed2_1>500)speed2_1=500;if(speed3_1>500)speed3_1=500;if(flogPV1==1)speed1=speed1_1*5;if(flogPV2==1)speed2=speed2_1*5;if(flogPV3==1)speed3=speed3_1*5;}/*****************************SCI读取，判断那种运动形式*************************/voidSciRed(){unsignedi2=0;if(SCI_RXDATA1[0]==42&&flog==1){if(SCI_RXDATA1[1]==35)//合成平移21\n基于DSP的全向运动控制系统软件设计{for(i2=2;i2<13;i2++){RXDINT[i2]=SCI_RXDATA1[i2]-48;//对SCI接收的进行处理}if(flogH==0||flogF==1||flogX==1){//将运动显示区，起始（x，y┖徒崾?x1,y1)部分清屏。起始0,83，结束128,143LCD_clear(0,63,128,64,0xff07,0xffe0);flogF=0;flogX=0;LCD_PutString(8,63,"合成运动;平移",0x001f,0x07e0);LCD_PutString(0,83,"速度:",0x001f,0x07e0);LCD_PutString(0,103,"角度:",0x001f,0x07e0);}else;SCIHspeed=RXDINT[2]*100+RXDINT[3]*10+RXDINT[4];//平移速度SCIHangle=RXDINT[6]*100+RXDINT[7]*10+RXDINT[8];//平移角度，RXDINT[5]为顺时针逆时针，1为顺2为逆SCIHag_SN=RXDINT[5]*1000+RXDINT[6]*100+RXDINT[7]*10+RXDINT[8];flogHStop=RXDINT[9];if(RXDINT[5]==1)flogangle=SCIHangle*2*PI/360;//将角度值进行变换如180°变换π3.14else{angle=360-SCIHangle;flogangle=angle*2*PI/360;}//将角度值进行变换如180°变换π3.14SCIH();//合成运动子程序执行函数flog=0;//传输接收标志位1为接收到，0为没有接收到。此处清零。}elseif(SCI_RXDATA1[1]==64)//旋转{for(i2=2;i2<13;i2++){RXDINT[i2]=SCI_RXDATA1[i2]-48;//对SCI接收的进行处理}if(flogX==0||flogF==1||flogH==1){LCD_clear(0,63,128,64,0xff07,0xffe0);LCD_PutString(8,63,"旋转运动",0x001f,0x07e0);LCD_PutString(0,83,"速度:",0x001f,0x07e0);LCD_PutString(0,103,"角度:",0x001f,0x07e0);flogF=0;flogH=0;}flogXSN=RXDINT[2];//顺时针为1，逆时针为2SCIXspeed=RXDINT[6]*100+RXDINT[7]*10+RXDINT[8];//平移速度if(flogXSN==1)SCIXangle=RXDINT[3]*100+RXDINT[4]*10+RXDINT[5];//平移角度elseif(flogXSN==2)SCIXangle=360-(RXDINT[3]*100+RXDINT[4]*10+RXDINT[5]);//平移角度SCIX();//旋转运动子程序执行函数flog=0;//传输接收标志位1为接收到，0为没有接收到。此处清零。}else//分解{21\n基于DSP的全向运动控制系统软件设计SCIF();//分解运动子程序执行函数flog=0;//传输接收标志位1为接收到，0为没有接收到。此处清零}flogPID=1;//标志位每改变运动形式起始速度为0，}}/******************测速函数***************************/voidspeedPV(){unsignedintsum5_3=0,sum6_3=0,sum1_3=0,i=0,j=0,PID=0;if(flogPV==1){flogPV=0;if(PVlb==3)PVlb=0;tt5_3[PVlb]=tt5;tt6_3[PVlb]=tt6;tt1_3[PVlb++]=tt1;tt5=0;tt1=0;tt6=0;for(i=0;i<3;i++){sum5_3+=tt5_3[i];sum6_3+=tt6_3[i];sum1_3+=tt1_3[i];}tt55=sum5_3/3;tt66=sum6_3/3;tt11=sum1_3/3;PIDPV();CY_26_read++;if(CY_26_read>10){CY_26_read=0;CY_26_Angle=CY_26_Angle_Out();if(flogH==1)SCIH();if(flogX==1)SCIX();tt5=0;tt1=0;tt6=0;}}if(pvlcd>305)//给上位机发送检测数值{pvlcd=0;scitt9[0]=tt55/100;scitt9[1]=tt55%100/10;scitt9[2]=tt55%10;scitt9[3]=tt66/100;scitt9[4]=tt66%100/10;scitt9[5]=tt66%10;21\n基于DSP的全向运动控制系统软件设计scitt9[6]=tt11/100;scitt9[7]=tt11%100/10;scitt9[8]=tt11%10;for(j=0;j<9;j++){SCI_Send(scitt9[j]);}LCD_Putnum(64,143,CY_26_Angle,0x001f,0x07e0);}}/******************SCI中断**********************/voidinterruptc_int1(){if(PIVR!=0x06)//高优先级的SCI接收中断{asm("CLRCINTM");return;}SCI_RXDATA=SCIRXBUF;//(SCIRXBUF)&0x00FF;//串口接收的数据保存//if(i==0&&SCI_RXDATA==42){SCI_RXDATA1[i++]=SCIRXBUF;SCIn=1;}elseif(SCIn==1){SCI_RXDATA1[i++]=SCIRXBUF;if(SCI_RXDATA1[i-1]==42){i=0;SCIn=0;flog=1;}}IFR=0x0001;asm("CLRCINTM");}/****************定时器1中断执行程序***********************/interruptvoidc_int2(){if(PIVR!=0x27){asm("CLRCINTM");return;}T1CNT=0;t0++;pvlcd++;if(t0==PVtime)//10MS速度检测{flogPV=1;t0=0;}EVAIFRA=EVAIFRA|0x0080;asm("CLRCINTM");21\n基于DSP的全向运动控制系统软件设计}/****************定时器4中断执行程序***********************/voidinterruptc_int4(){switch(PIVR){case0x0038:EVBIFRC=EVBIFRC|0x0004;tt6++;break;case0x0037:EVBIFRC=EVBIFRC|0x0002;tt5++;break;case0x0033:EVAIFRC=EVAIFRC|0x0001;tt1++;break;default:IFR=IFR|0x0008;break;}asm("CLRCINTM");}/****************无效中断执行程序***********************/voidinterruptPHANTOM(){asm("clrcINTM");//返回前开中断return;}致谢21\n基于DSP的全向运动控制系统软件设计首先感谢翟天嵩老师，本设计是在翟老师的细心指导下完成的，对我整个毕业设计理论知识和论文的撰写倾注了大量的心血。还有感谢xxx老师、xxx老师、xxx老师以及所有培养过我的老师，你们不但让我学习了详细的理论知识，更重要的是为我营造了一种精神氛围，对我的人生成长有着重要意义。同时还有xxxxx，谢谢他们对我毕业设计及四年的大学生活的帮助。四年的大学生活转瞬即逝，我很自豪我来到了xxxx学院，来到了电子系。是你们教会我如何做学问，如何建立一个团队，如何去做人。这是我一生的财富，在此请接受我诚挚的谢意！21