毕业设计（论文）-基于dsp的全向运动控制系统软件设计

基于DSP的全向运动控制系统软件设计I基于DSP的全向运动控制系统软件设计摘要本文基于DSPC2000系列TMS320LF2407A核心控制芯片，以CCSTUDIOV33软件为开发平台，在了解ROBOCUP中型组足球机器人和其他全向机器人的基础上，主要完成了全向运动控制系统的软件设计。通过对全向运动控制系统的研究，建立了三轴全向运动学数学模型，并对整体平移运动、原地旋转运动、边平移边旋转三种运动方式进行分析和数学建模；利用上位机，经无线模块发送运动方式和各种运行参数，控制运动系统实现各种运动模式；经全向运动控制系统软件的编写和与相关硬件联机调试，系统实现了预订的全向运动形式。通过试验验证和结果分析，各种运动状态的准确性已达到性能的基本要求。关键词DSP；全向运动控制；数学建模；MATLAB仿真；串行通信软件设计SOFTWAREDESIGNOFOMNIDIRECTIONALMOTIONCONTROLSYSTEMBASEDONDSPABSTRACTTHISARTICLEISBASEDONTHETMS320LF2407AINTHESERIESOFDSPC2000ASTHECONTROLCORECHIPANDTHESOFTWAREOFCCSTUDIOV33ASTHEDEVELOPMENTPLATFORMITISBASISONTHEUNDERSTANDINGOFMEDIUMSIZEDGROUPABOUTSOCCERROBOTINROBOCUPANDOTHEROMNIDIRECTIONALROBOT,MAINLYACCOMPLISHTHESOFTWAREDESIGNOFAOMNIDIRECTIONALMOTIONCONTROLSYSTEMTHROUGHTHERESEARCHTOTHEMOTIONCONTROLSYSTEM,MOTIONCONTROLSYSTEMWASESTABLISHEDTOALLTHREEAXISKINEMATICSMATHEMATICMODEL,BUILDTHEMATHEMATICALMODELINGANDANALYSISTHREEKINDOFMOVEMENT,WHICHINCLUDETHEWHOLETRANSLATIONMOVEMENT,SPINAROUNDMOVEMENTANDEDGETRANSLATIONANDROTATINGUSINGTHEPC,THEWIRELESSMODULETOSENDTHEMOVEMENTANDAVARIETYOFOPERATINGPARAMETERS,CONTROLMOTIONSYSTEMTOACHIEVEAVARIETYOFMOTIONMODESBYTHEWHOLEMOTIONCONTROLSYSTEMSOFTWARETOWRITEANDONLINECALIBRATIONWITHRELEVANTHARDWARE,THESYSTEMREALIZEDTHEOMNIDIRECTIONALMOVEMENTWEEXPECTEDTHROUGHTHERESULTSANALYSISANDEXPERIMENTALVERIFICATION,THEACCURACYHAVEALREADYMEETREQUIREMENTSOFVARIOUSSPORTSSTATEKEYWORDSDSPOMNIDIRECTIONALMOTIONCONTROLMATHEMATICALMODELINGMATLABSIMULATIONSERIALCOMMUNICATION；SOFTWAREDESIGN基于DSP的全向运动控制系统软件设计II目录1引言111全向运动控制系统发展现状112本课题的研究意义及前景213论文组织结构32全向运动控制系统分析321全向运动控制系统运动学模型建立322不同运动方式的运动特性5221平移运动5222原地旋转运动7233边平移边旋转运动83基于DSP的硬件系统简介831控制芯片选择832硬件系统结构图933硬件系统基本模块104系统运动控制的MATLAB仿真1341电机PID控制1342转速检测1443MATLAB仿真155系统软件设计1651软件开发平台及仿真器1652运动控制软件设计18521主程序18522三种基本运动状态子程序19523电机控制子程序20524无线发送子程序2053上位机软件及通讯协议216实验验证及结果分析2261试验场地2362各项性能测试24621速度PID测试24基于DSP的全向运动控制系统软件设计III622平移运动测试24623原地旋转测试25624平移旋转运动测试2663影响因素分析26结束语28参考文献29附录30致谢42基于DSP的全向运动控制系统软件设计11引言随着机器人技术的日新月异，机器人应用领域也已从工业走向普通生活。机器人技术已经在人们的生活中越来越发挥着重要的作用。作为机器人中的全向运动机器人，有着其特殊的运动形式，即可以不改变姿势向任何方向运动，找到最佳位置。同时也可以边往某个方向运动，边调整自身的姿势达到最佳运动效果，可以在平面内完美的往任何方向做三自由度运动。全向运动机器人其高灵活度，势必会在机器人领域发挥不可代替的作用。在全向运动机器人中现阶段比较常见的是三轮结构和四轮结构（如图1所示）。通过控制各个轮子间相互协调运动达到全方位的运动效果，包括全方位平移、全方位边平移边旋转和原地旋转运动形式。要完整文章毕业论文的扣扣（1499088324）去掉中间的横线图1三轮和四轮的结构模型2全向运动控制系统分析21全向运动控制系统运动学模型建立在引言中介绍了全向运动控制的几种结构，本设计选用三轮结构构建全向运动系统34。其结构图和实物付如图4所示。基于DSP的全向运动控制系统软件设计2A结构图B实物图图4全向结构图和实物图由图5中B所示，V为机器人系统整体速度，世界坐标XY坐标系中X轴与整体速度V的逆时针夹角为，在世界坐标系中将整体速度V分解成沿X轴和Y轴的速度分别为和。XY22不同运动方式的运动特性全向运动的运动形式大体可分为平移运动、原地旋转运动、边平移边旋转运动三种运动形式。三种运动形式有着不同的运动特性，以下将分三小节对三种运动形式进行单独分析。221平移运动平移是基本的也是比较常用的运动形式。由于是平移不考虑旋转运动，我们将模型进行简化，其简化后运动学模型图如图6所示。A平移模型B系统整体平移模型基于DSP的全向运动控制系统软件设计3图6简化后运动运动学模型单独考虑平移运动故将式3进行简化为式4413122030XYLVV假设小车整体平移矢量速度为V，与X轴成角。所以我们可以分解为和，XVY其分解公式为式5。5COSINXYV由式4和式5我们可以得到三个驱动轮的速度，见式6。61122333SINCOSIN62COSISSIVV驱动轮速度、的速度范围都是。的值则由电机性能决123MAXAVMAX定。通过式6我们可以反向推导出平移速度V的速度范围。如图7，其中圆是以驱动轮最大速度大小为半径的圆，红色速度V与X轴的角度为，随着由MAXV0360变化V的轨迹为红色正六边形。其中最大速度为。MAX23V基于DSP的全向运动控制系统软件设计4图7平移最大速度模型3基于DSP的硬件系统简介31控制芯片选择作为运动控制系统的核心控制芯片，根据以往在实验室的经验，其核心控制芯片当属DSPC2000系类。C2000系类对于运动控制有着诸多的优点，本设计采用C2000系类TMS320LF2407A的控制板。TMS320LF2407A主要特点如下32硬件系统结构图通过第2章关于运动学模型的建立，为使得系统能够按照建立的数学模型进行运动，除了31节的DSP主控制芯片外还需要其它的硬件的支持，本课题设计的硬件系统结构图如图9所示。其主要的工作方式为PC上位机选择相应的运动方式，经过RS232协议连接无线模块，由无线模块发送数据。DSP下位机的无线模块接收上位机发送的数据，经RS232通信协议发送给DSP。DSP接收到数据进行相应的处理，分析出要进行的运动形式及各种参数，同时利用I2C协议采集指南针的数据，将这些信息通过第2章的全向运动学模型分析的各种公式转换成各个电机的速度值。为使得电机能够稳、准、快的执行，本设计采用经典PID控制，以增量式编码器采集的值为反馈。在进行的过程中液晶模块实时显示运动方式和参数及指南针的值，同时将编码器的值通过无线模块传送给PC上位机。DSP下位机将按照程序设定执行上位机设定的运动方式，只有上位机重新发送新的运动方式才会改变。33硬件系统基本模块根据图9硬件系统结构图所示，本节将分析几个重要的模块。分为电源模块、无线通信模块、指南针模块、液晶显示模块，DSP下位机模块、电机及驱动模块和全向轮。其中DSP下位机31节已经介绍，在此不再叙述。要完整文章毕业论文的扣扣（1499088324）去掉中间的横线4系统运动控制的MATLAB仿真本设计的全向运动控制系统使用三轴全向控制，即控制3个电机相互协调运动来实现全向轮动。电机的控制对整个系统是至关重要的。本章主要分析系统的PID控制和MATLAB仿真。基于DSP的全向运动控制系统软件设计541电机PID控制电机是运动系统的执行机构，不管DSP计算的多么正确，但是电机执行达不到要求，同样系统是无法实现全向运动的。为使得电机能够稳、准、快的执行，而PID是根据偏差的比例、积分、微分的线性组合进行反馈控制，是迄今为止工业中应用最为广泛的一种控制方法。所以本设计采用PID分别对三个电机进行闭环控制。42转速检测本设计选用增量式PID即41节中所阐述的PID。对于PID控制反馈必不可少，在本设计使用的是FAULHABER带双路编码器减速电机，即增量式光电编码器来做为反馈环节，其工作原理如图14。光源经过码盘和检测光栅照射的信号通过光电检测器件检测，检测出的正弦值经转换电路转换成矩形波，最后出来A相B相Z相。A相B相相差90度，通过软件能够很好的得到此时电机的速度和方向。对AB相的采集是电机闭环控制的反馈量，关乎电机PID控制是否能达到稳准快的要求。本设计使用输入捕捉CAP功能对脉冲进行采集，将采集量进行相应处理即得到PID控制的反馈。43MATLAB仿真在三轴控制系统中，PID是应用最广泛的控制算法。针对电机模型，这里设定电机的模型为，针对此电机模型进行MATLAB仿真，观察PID控制算234GSS法的控制效果1011。控制结果比较曲线如图15所示。绿色曲线为期望值，蓝色曲线为PID控制跟踪曲线，红色曲线为误差曲线，实际速度可以通过编码器反馈。可以看出PID控制方式的响应速度、跟踪精度可以达到系统要求。要完整文章毕业论文的扣扣（1499088324）去掉中间的横线5系统软件设计根据前几章模型建立分析和硬件系统构建，本章主要介绍全向运动控制系统程序编写。前两节介绍了下位机全向运动系统使用的程序编写平台软件和整个系统流程图及各部分子程序的流程图。最后一节介绍上位机部分，编写上位机软件部分和上位机下位机使用的通讯协议。51软件开发平台及仿真器本设计的核心控制芯片为TMS320LF2407A，其软件开发平台使用官方工具CCSV33，仿真器选用XDS510USB20。安装CCSV33后生成两个部分，即CCSTUDIO基于DSP的全向运动控制系统软件设计6V33和SETUPCCSTUDIOV33。其中利用SETUPCCSTUDIOV33对开发工具参考硬件进行相应配置，利用CCSTUDIOV33建立工程编写软件,其开发周期示意图为图18所示12。图19即为CCSTUDIOV33开发环境。52运动控制软件设计由于本课题是基于DSP全向运动控制系统软件设计，侧重控制全向运动，故本设计将检测部分做了精简，只需要采集指南针和编码脉冲。其全向运动方式经过上位机无线传输指令，然后DSP接收指令进行处理最后控制电机协调运动，根据指南针和编码器进行闭环控制，以达到最佳效果。6实验验证及结果分析为验证本设计的各种全向运动是否准确，本章将对各种全向运动形式进行试验验证。设计试验场地，记录测试试验数据，对结果进行分析。61试验场地由于本设计使用的驱动轮为尼龙材质。驱动轮与地面接触部分硬度较大，如果在普通地板上驱动轮将得不到足够大的摩擦力，有严重的“打滑”现象。为提供足够摩擦力结合实验室条件，选择飞思卡尔赛道使用的KT板，做成120CM100CM的试验场地，并在场地上标上边距为20CM的方格，同时为验证转角正确性标上了相隔45的虚线，结构图见图26。62各项性能测试需要试验验证测试的有PID验证、平移验证、原地旋转验证、边平移边旋转运动。设计制作试验装置，分别对各种性能测试。63影响因素分析影响系统性能的因素大部分来源于外部，大致分为以下几种情况（1）电池电量不足，导致系统电机启动或大的变化时电流不足。导致系统不能很好的按照设定的形式运动。2）外部磁场干扰，数字指南针能够很灵敏的感应磁场，所以外部稍强点的磁场就能干扰系统，一方面是金属铁，其很容易磁化，导致变成弱磁铁。在室内钢筋结构的环境下各个位置受到的磁干扰不同，经测试最大可影响角度达30角，严重影响了系统性能。基于DSP的全向运动控制系统软件设计7另一方面是电机，由于电机带有磁性较强磁铁，产生磁场的磁场也对指南针有一定干扰。去除干扰的方法只能经过硬磁补偿来减少很少部分干扰，大多数干扰无法去除。3）驱动轮干扰，本设计使用的是瑞典全向轮，轮子外侧有两排相互交错的侧轮，使得驱动轮滚动过程中，造成上下震动。同时由于驱动轮滚动会造成电机力矩发生变化，L12或145CM，及内外两个侧滑轮着地时会造成力矩25CM的变化，会使得系统转动发生误差。要完整文章毕业论文的扣扣（1499088324）去掉中间的横线结束语全向运动控系统在全向机器人上的应用可实现机器人向任意方向做直线运动而不需要事先作旋转运动，并且在以直线运动到达目标点的过程中同时可以做自身旋转运动，以调整机器人的姿态，从而达到终态所需的姿态角。其特殊的移动方式使得全向机器人越来越得到重视和应用，其未来的实用领域和前景将更加宽广。本设计完成的主要工作为以下几点1对目前的几种运动结构进行了研究分析，设计了互成120度的三轴运动结构。2对三轴运动控制结构进行了数学建模，进行了运动学分析。对全向运动的运动的几种运动形式进行了详细的分析计算。3运用TMS320LF2407A芯片为控制核心，使用了其模块UART（SCI）、IIC（模拟）、PWM、I/O、CAP、外部扩展RAM。同时围绕核心的外部器件进行程序的编写调试，已达到全向控制的目的。4使用了VC60的MSCOMM控件编写上位机软件，设计上位机界面和功能，编写通信协议。使得系统能够经过选择相关运动形式，修改上位机的相关参数来实现运动系统的全向运动。同时能够在上位机显示系统现在运动的状态和三个电机速度值。并实现了通过游戏手柄来遥控系统的各种运动状态。5对多电机实现了数字增量式PID控制，三个电机各自PID控制且互不影响，并通过MATLAB进行了仿真。使得计算出的速度能够很好的执行，提高了系统的准确性。6实现了全向运动控制系统的所有运动形式，且经过试验测试验证了其准确性。参考文献基于DSP的全向运动控制系统软件设计81徐旭,李实,叶榛,孙增圻ASURVEYROBOCUPANDTHERESEARCHPROCEEDINGSOFTHE3RDWORLDCONGRESSONINTELLIGENTCONTROLANDAUTOMATIONM,20082赵冬斌,易建强,邓旭玥全方位移动机器人结构和运动分析J机器人2003253海丹全向移动平台的设计与控制D长沙国防科技大学20094龚建伟VISUALC/TURBOC串口通信编程实践北京电子工业出版社，20045RAULROJASOMNIDIRECTIONALCONTROL,FREIEUNIVERSITYBERLIN,TECHNICALREPORTB1003,FUBEDINJUNE20036刘和平DSP原理及电机控制应用基于TMS320LF240X系列M北京北京航空航天大学出版社,20087王茂飞,程昱TMS320C2000DSP技术与应用开发M北京北京清华大学出版社,20078TEXASINSTRUMENTSTMS320LF/LC240XADSPCONTROLLERSREFERENCEGUIDESYSTEMANDPERIPHERALSSPRU357BMDECEMBER,20019陶永华，尹怡欣，葛芦生新型PID控制及其应用M北京机械工业出版社199810薛定宇控制系统计算机辅助设计MATLAB语言与应用（第2版）M北京清华大学出版社,200611范海艇基于NIOS的中型足球机器人底层控制系统的研究与实现D上海,上海大学12姜艳波数字信号处理器DSP应用100例M，北京化学工业出版社，200713海丹全向移动平台的设计与控制D长沙国防科技大学，200614张宏林精通VISUALC串口通信技术与工程实践第3版北京人民邮电出版社，200815龚建伟VISUALC/TURBOC串口通信编程实践（第二版）M,北京电子工业出版社，20079附录基于DSP的全新运动控制系统软件清单如下基于DSP的全向运动控制系统软件设计9其中自己建的有GB161H（存储液晶字模数据），24C02C（操作指南针），DESIGNC（运动控制策略），ILI9163C（液晶显示），MAINC（主程序）。由于程序比较大，在此只附录上MAINC和DESIGNC两个文件。MAINC/FILENAMEMAINC/AUTORLIHAIQING/DATE04/01/2011/DESCRIPTIONTHISFILEPROVIDESMAINFUNCTION,PROVIDEINITIALIZATION/串口发送与中断接收程序,波特率设置为9600/IO口占用PE1PE6（PWM）PA3PA6和PB0PB7和PE7液晶屏使用/PF0CAP5,PF1CAP6,PA3CAP1三电机测速。PE2PWM8PE4PWM10，PE6PWM12三电机/PC01PF23PF45电机正反转/INCLUDE“GLOBALC“INCLUDE“MATHH“VOIDSYSTEMINITVOIDTIMER1INITVOIDILI9163_INITVOIDKICKDOGVOIDPWM_INITVOIDIOINITVOIDTIMER4INITVOIDTIMER2INIT/基于DSP的全向运动控制系统软件设计10/主程序/MAINSYSTEMINIT/系统初始化IOINIT/IO口初始化SCI_INIT/SCI初始化PWM_INIT/PWM初始化ILI9163_INIT/液晶初始化TIMER1INIT/定时器1初始化/TIMER4INIT/定时器4初始化/TIMER2INIT/定时器2初始化/ASM“CLRCINTM“DISPLAY0XFF07,0XFFE0LCD_PUTSTRING40,03,“李海清“,0X001F,0X07E0/液晶界面显示信息LCD_PUTSTRING16,23,“运动控制系统“,0X001F,0X07E0/LCD_PUTSTRING0,143,“指南针“,0X001F,0X07E0/WHILE1SCIRED/判断并读取SCI数据SPEEDPV/速度函数/系统初始化/VOIDSYSTEMINITASM“SETCINTM“/关闭总中断/ASM“CLRCSXM“/禁止符号位扩展/ASM“CLRCCNF“/B0块映射为ONCHIPDARAM/ASM“CLRCOVM“/累加器结果正常溢出/SCSR10X834E/系统时钟CLKOUT20240M/打开ADC,EVA,EVB,CAN和SCI的时钟/WDCR0X006F/禁止看门狗,看门狗时钟64分频/KICKDOG/初始化看门狗/IFR0XFFFF/清除中断标志/IMR0X000B/打开中断1,2/IO口初始化/VOIDIOINITMCRCMCRC/PE0PE7，PF0PF5有效0000MCRAMCRA/PA0PA7，PB0PB7有效0208MCRBMCRBPADATDIR0XF708/PA口初始化值为0PBDATDIR0XFF00/PB口初始化值为0基于DSP的全向运动控制系统软件设计11PEDATDIR0XFF7F/PE口初始化值为0PFDATDIR0XFC33/PF05为1PF6输入PCDATDIR0XFFFF/PC输出PC7PC001010101/PWM初始化/VOIDPWM_INITMCRCMCRC|0X0054/PE1PE6为PWM口7EEVBIFRA0XFFFF/清除中断标志ACTRB0X0666/PWM12,10,8低有效,PWM11,9,7高有效/DBTCONA0X0530/使能死区定时器1,分频40M/1625M,死区时间504US2UST3PR2500/定时器3周期值,定时04US25001MS/2500CMPR40/比较值CMPR50/比较值CMPR60/比较值COMCONB0XA600/比较控制寄存器T3CNT0/定时器3计数器清零EVBIMRA0X0080/定时器3周期中断使能T3CON0X144E/增模式,TPS系数40M/1625M,T3使能,VOIDKICKDOG/踢除看门狗/WDKEY0X5555WDKEY0XAAAADESIGNC/FILENAMEDESIGNC/AUTORLIHAIQING/DATE03/10/2011/DESCRIPTIONTHISFILEPROVIDESMOTIONCONTROLSTRATEGY/INCLUDE“GLOBALC“INCLUDE“MATHH“DEFINESPEED1CMPR4/宏定义SPEED1电机1速度DEFINESPEED2CMPR5/宏定义SPEED1电机2速度DEFINESPEED3CMPR6/宏定义SPEED1电机2速度DEFINEPVTIME3/扫描时间，每个轮子检测PVTIME个MSDEFINEKP08/宏定义KPDEFINEKI02/宏定义KIDEFINEKD02/宏定义KDDEFINEPI314159/宏定义PI基于DSP的全向运动控制系统软件设计12UNSIGNEDINTFLOG0,FLOGF0,FLOGH0,FLOGX0,FLOGPID0/各种运动状态标志位UNSIGNEDINTFLOGPV0,PVLCD0,FLOGPV10,FLOGPV20,FLOGPV30UNSIGNEDINTSCI_RXDATA,RXDINT140,0,0,0,0,0,0,0,0,0,0,0,0,0INTSUM110,SUM220,SUM330INTSUM11_00,SUM22_00,SUM33_00INTSCIN0,I0,SCISP1,SCISP2,SCISP3INTSPEED1_10,SPEED2_10,SPEED3_10,SPEED1_00,SPEED2_00,SPEED3_00INTTT50,TT60,TT10,T00,TT5_330,0,0,TT6_330,0,0,TT1_330,0,0UNSIGNEDINTTT550,TT660,TT110/电机编码器值TT55TT66TT11分别电机13的离散值UNSIGNEDINTSCITT990,0,0,0,0,0,0,0,0UNSIGNEDINTPVLB0,CY_26_READ0,CY_26_ANGLE0UNSIGNEDINTSCIHSPEED0,SCIHANGLE0,SCIHAG_SN0UNSIGNEDINTSCIXSPEED0,SCIXANGLE0UNSIGNEDINTFLOGHSTOP0,FLOGXSN0FLOATFLOGANGLE00,ANGLE1100,ANGLE2200,ANGLE3300,ANGLE00CHARSCI_RXDATA1140,0,0,0,0,0,0,0,0,0,0,0,0,0/SCI初始化/VOIDSCI_INITMCRAMCRA|0X0003/IOPA0,IOPA1为串口SCICCR0X0007/空闲多处理器模式，8位数据位,1位停止位，无奇偶校验位SCICTL10X0003/接收，发送，内部时钟使能，SLEEP1SCICTL20X0002/接收中断使能SCIPRI0X0000/高中断优先级SCIHBAUD0X0002/40M,波特率设置为9600SCILBAUD0X0008SCICTL10X0023/串口初始化完成/定时器1初始化/VOIDTIMER1INITT1PR2500/定时器1初值,定时04US25001MST1CNT0GPTCONA0X0000EVAIMRAEVAIMRA|0X0080/定时器1周期中断使能EVAIFRAEVAIFRA|0XFFFF/清除中断标志T1CON0X144E/增模式,TPS系数40M/1625M,T1使能/定时器12初始化/VOIDTIMER2INITT2PR2500T2CNT0WSGR0X0000CAPCONA0X2240CAPFIFOA0X0100基于DSP的全向运动控制系统软件设计13EVAIMRCEVBIMRC|0X0001EVAIFRCEVBIFRC|0XFFFFT2CON0X1440/1440/定时器4初始化输入捕捉/VOIDTIMER4INITT4PR2500T4CNT0WSGR0X0000CAPCONB0X3014CAPFIFOB0X1500EVBIMRCEVBIMRC|0X0006EVBIFRCEVBIFRC|0XFFFFT4CON0X1440/1440/SCI发送/VOIDSCI_SENDUNSIGNEDDATA/发送0X800X87,共8个字节数据SCITXBUFDATAWHILESCICTL2/SCI接收分解运动数据/VOIDSCIF/执行速度分解程序，分别控制三个电机的速度UNSIGNEDINTI1FORI1I360HSDANGLEHSDANGLE360HSD_SCIANGLEHSDANGLECY_26_ANGLE/设定角度与实际角度的差值ABSANGLEABSHSD_SCIANGLE/设定与实际差距的绝对值IFRXDINT101IFABSANGLE180IFHSD_SCIANGLE358ELSECYSPEED0IFABSANGLE180CYSPEED15ELSE/防止抖动IFABSANGLE1ELSECYSPEED0ELSECYSPEED0CYANGLESDPI/2FLOGANGLEIFFLOGANGLE0PCDATDIRPCDATDIRELSEPCDATDIRPCDATDIRIFSUM22_00PFDATDIRPFDATDIRELSEPFDATDIRPFDATDIRIFSUM33_00PFDATDIRPFDATDIRELSEPFDATDIRPFDATDIRSUM11ABSSUM11_0SUM22ABSSUM22_0SUM33ABSSUM33_0IFFLOGHSTOP1SUM110SUM220SUM330ELSEIFSUM110PCDATDIRPCDATDIR|0XFFFFFLOGPV10ELSEFLOGPV11IFSUM220PFDATDIRPFDATDIR|0XFC0CFLOGPV20ELSEFLOGPV21IFSUM330PFDATDIRPFDATDIR|0XFC30FLOGPV30ELSEFLOGPV31IFFLOG1LCD_PUTNUM48,83,SCIHSPEED,0X001F,0X07E0LCD_NUM48,103,SCIHAG_SN,0X001F,0X07E0FLOGH1/合成平移运动标志位，控制显示部分清屏，显示相应运动方式和数据/SCI接收旋转运动数据/VOIDSCIX/执行旋转运动，控制旋转INTSDANGLE0INTSD_SCIANGLE0,XSPEED0,ABSANGLE0SDANGLESCIXANGLE180IFSDANGLE360SDANGLESDANGLE360SD_SCIANGLESDANGLECY_26_ANGLEABSANGLEABSSD_SCIANGLEIFABSANGLE180IFSD_SCIANGLE358ELSEXSPEED0IFABSANGLE180XSPEED15ELSE/防止抖动IFABSANGLE1ELSEXSPEED0SUM11_0XSPEEDSUM22_0XSPEEDSUM33_0XSPEEDPCDATDIR0XFFFFPFDATDIRPFDATDIR|0XFCFCIFSUM11_00PCDATDIRPCDATDIRELSEPCDATDIRPCDATDIRIFSUM22_00PFDATDIRPFDATDIRELSEPFDATDIRPFDATDIRIFSUM33_00PFDATDIRPFDATDIRELSEPFDATDIRPFDATDIR基于DSP的全向运动控制系统软件设计16SUM11ABSSUM11_0SUM22ABSSUM22_0SUM33ABSSUM33_0IFSUM110PCDATDIRPCDATDIR|0XFFFFFLOGPV10ELSEFLOGPV11IFSUM220PFDATDIRPFDATDIR|0XFC0CFLOGPV20ELSEFLOGPV21IFSUM330PFDATDIRPFDATDIR|0XFC30FLOGPV30ELSEFLOGPV31IFFLOG1LCD_PUTNUM48,83,SCIHSPEED,0X001F,0X07E0LCD_NUM48,103,SCIHAG_SN,0X001F,0X07E0FLOGX1/旋转运动标志位，控制显示部分清屏，显示相应运动方式和数据/速度PID/INTEE10,EE20,EE30,EE1_20,EE1_10,EE2_20,EE2_10,EE3_20,EE3_10VOIDPIDPVEE1_2EE1_1EE1_1EE1EE2_2EE2_1EE2_1EE2EE3_2EE3_1EE3_1EE3EE1SUM11TT55EE2SUM22TT66EE3SUM33TT11IFFLOGPID1SPEED1_10SPEED2_10SPEED3_10FLOGPID0/每改变运动形式速度变0SPEED1_1SPEED1_1KPEE1EE1_1KIEE1KDEE12EE1_1EE1_2/KPKIKDSPEED2_1SPEED2_1KPEE2EE2_1KIEE2KDEE22EE2_1EE2_2SPEED3_1SPEED3_1KPEE3EE3_1KIEE3KDEE32EE3_1EE3_2