基于DSP的全向运动控制系统软件设计说明

1、基于DSP的全向运动控制系统软件设计摘要基于DSP C2000系列TMS320LF2407A核心控制芯片和CCStudio V3.3软件为开发平台，在了解RoboCup中型足球机器人等全向机器人的基础上，进行全向运动控制系统的软件设计主要完成。 .通过对全向运动控制系统的研究，建立了三轴全向运动学的数学模型，对整体平移运动、原地旋转运动、自转运动三种运动模式进行了分析和数学建模。在进行翻译时；使用上位机，通过无线模块致运动模式和各种运行参数，控制运动系统实现各种运动模式；通过全向运动控制系统软件的编写和相关硬件的在线调试，系统实现了预定的全向运动形式。通过实验验证和结果分析，各种运动状态的精度

2、都达到了性能的基本要求。关键词 DSP；全向运动控制；数学建模； MATLAB仿真；串行通信；软件设计目录 TOC o 1-3 h z u HYPERLINK l _Toc292193127 1简介 PAGEREF _Toc292193127 h 1 HYPERLINK l _Toc292193128 1.1全向运动控制系统发展现状 PAGEREF _Toc292193128 h 1 HYPERLINK l _Toc292193129 1.2本课题的研究意义与展望 PAGEREF _Toc292193129 h 2 HYPERLINK l _Toc292193130 1.3论文组织结构 PAG

3、EREF _Toc292193130 h 3 HYPERLINK l _Toc292193131 2全向运动控制系统分析 PAGEREF _Toc292193131 h 3 HYPERLINK l _Toc292193132 2.1全向运动控制系统运动学模型的建立 PAGEREF _Toc292193132 h 3 HYPERLINK l _Toc292193133 2.2不同运动方式的运动特点 PAGEREF _Toc292193133 h 5 HYPERLINK l _Toc292193134 2.2.1平移运动 PAGEREF _Toc292193134 h 5 HYPERLINK l

4、_Toc292193135 2.2.2原位旋转运动 PAGEREF _Toc292193135 h 7 HYPERLINK l _Toc292193136 2.3.3平移时旋转 PAGEREF _Toc292193136 h 8 HYPERLINK l _Toc292193137 3 基于DSP的硬件系统介绍 PAGEREF _Toc292193137 h 8 HYPERLINK l _Toc292193138 3.1控制片选 PAGEREF _Toc292193138 h 8 HYPERLINK l _Toc292193139 3.2硬件系统结构图 PAGEREF _Toc292193139

5、 h 9 HYPERLINK l _Toc292193140 3.3硬件系统基本模块 PAGEREF _Toc292193140 h 10 HYPERLINK l _Toc292193141 4 MATLAB系统运动控制仿真 PAGEREF _Toc292193141 h 13 HYPERLINK l _Toc292193142 4.1电机PID控制 PAGEREF _Toc292193142 h 13 HYPERLINK l _Toc292193143 4.2速度检测 PAGEREF _Toc292193143 h 14 HYPERLINK l _Toc292193144 4.3 MATLA

6、B仿真 PAGEREF _Toc292193144 h 15 HYPERLINK l _Toc292193145 5系统软件设计 PAGEREF _Toc292193145 h 16 HYPERLINK l _Toc292193146 5.1软件开发平台和模拟器 PAGEREF _Toc292193146 h 16 HYPERLINK l _Toc292193147 5.2运动控制软件设计 PAGEREF _Toc292193147 h 18 HYPERLINK l _Toc292193148 5.2.1主程序 PAGEREF _Toc292193148 h 18 HYPERLINK l _T

7、oc292193149 5.2.2三个基本运动状态子程序 PAGEREF _Toc292193149 h 19 HYPERLINK l _Toc292193150 5.2.3电机控制子程序 PAGEREF _Toc292193150 h 20 HYPERLINK l _Toc292193151 5.2.4无线致子程序 PAGEREF _Toc292193151 h 20 HYPERLINK l _Toc292193152 5.3 上位机软件及通讯协议 PAGEREF _Toc292193152 h 21 HYPERLINK l _Toc292193153 6实验验证与结果分析 PAGEREF

8、_Toc292193153 h 22 HYPERLINK l _Toc292193154 6.1测试场地 PAGEREF _Toc292193154 h 23 HYPERLINK l _Toc292193155 6.2各种性能测试 PAGEREF _Toc292193155 h 24 HYPERLINK l _Toc292193156 6.2.1速度PID测试 PAGEREF _Toc292193156 h 24 HYPERLINK l _Toc292193157 6.2.2平移运动测试 PAGEREF _Toc292193157 h 24 HYPERLINK l _Toc292193158

9、6.2.3原位旋转试验 PAGEREF _Toc292193158 h 25 HYPERLINK l _Toc292193159 6.2.4平移+旋转运动测试 PAGEREF _Toc292193159 h 26 HYPERLINK l _Toc292193160 6.3影响因素分析 PAGEREF _Toc292193160 h 26 HYPERLINK l _Toc292193161 结论 PAGEREF _Toc292193161 h 28 HYPERLINK l _Toc292193162 参考文献 PAGEREF _Toc292193162 h 29 HYPERLINK l _Toc

10、292193163 附录 PAGEREF _Toc292193163 h 30 HYPERLINK l _Toc292193164 至 PAGEREF _Toc292193164 h 421 简介随着机器人技术的飞速发展，机器人应用领域也从工业走向了日常生活。机器人技术在人们的生活中发挥着越来越重要的作用。作为机器人中的全向运动机器人，它有其特殊的运动形式，即可以在不改变姿态的情况下向任意方向运动，以找到最佳位置。同时，它还可以向某个方向移动并调整姿态，以达到最佳的运动效果，并且可以在平面上完美地进行任意方向的三自由度运动。全向运动机器人的高度灵活性必将在机器人领域发挥不可替代的作用。在全向运

11、动机器人中，现阶段以三轮结构和四轮结构较为常见（如图1所示）。通过控制各个车轮的协调运动，可以实现全方位的运动效果，包括全向平移、全向平移和旋转、原地旋转。完成毕业论文+按钮：（149-908-83-24）去掉中间的横线图1 三、四轮结构模型2 全向运动控制系统分析2.1 全向运动控制系统运动学模型的建立在引言中，介绍了全向运动控制的几种结构。在本设计中，三轮结构用于构建全向运动系统34 。其结构图和实物支付如图4所示。(a) 结构图 (b) 实物图图4 全方位结构图和物理图如图5(b)所示，V为机器人系统的整体速度，在世界坐标XY坐标系中X轴与整体速度V的逆时针夹角为。在世界坐标系中，整体速

12、度 V 被分解为沿 X 轴和 Y 轴的速度分别为和。2.2 不同运动方式的运动特点全向运动的运动形式大致可以分为三种运动形式：平移运动、原地旋转运动和边平移边旋转运动。三种运动形式具有不同的运动特性，下面将分三个小节分别对三种运动形式进行分析。2.2.1 平移运动平移是一种基本且常用的运动形式。由于是平移，不考虑旋转运动，我们对模型进行了简化，简化后的运动学模型图如图6所示。(a) 翻译模型 (b) 系统整体翻译模型图 6 运动学的简化运动学模型仅考虑平移运动，将公式（3）简化为公式（4）：(4)假设汽车的整体平移矢量速度为 V，与 X 轴形成夹角 。因此我们可以将其分解为sum ，其分解公式

13、为式（5）。(5)由式(4)和式(5)可以得到三个驱动轮的转速，见式(6)。(6)驱动轮的转速，和转速范围都是。该值由电机性能决定。由式(6)可以逆推导出平移速度V的速度范围。如图7所示，圆是以驱动轮最大速度为半径的圆，与红色速度V的夹角X轴为。V的轨迹从0变为360是一个红色的正六边形。其中最大速度为。图 7 平移最大速度模型3 基于DSP的硬件系统介绍3.1 控制片选作为运动控制系统的核心控制芯片，根据实验室以往的经验，其核心控制芯片属于DSP C2000系列。 C2000 系列在运动控制方面具有许多优势。本设计采用C2000系列TMS320LF2407A的控制板。 TMS320LF240

14、7A的主要特点如下：3.2 硬件系统结构图通过第 2 章运动学模型的建立，为了使系统按照建立的数学模型运行，除了第 3.1 节的 DSP 主控芯片外，还需要其他硬件支持。本项目设计的硬件系统结构图如图所示。如图 9 所示。其主要工作模式如下：PC主机选择相应的运动模式，通过RS232协议连接无线模块，无线模块致数据。 DSP下位机的无线模块接收上位机致的数据，通过RS232通讯协议致给DSP。 DSP接收数据并进行相应的处理，分析运动形式和要执行的各种参数，并使用I2C协议收集罗盘数据，并将这些信息转换成第2章全向运动学模型分析的各种公式。每个电机的速度值。为使电机运行稳定、准确、快速，设计采

15、用经典的PID控制，增量编码器采集的数值反馈。在此过程中，液晶模块实时显示罗盘的运动方式和参数及数值，同时通过无线模块将编码器的数值传送给PC主机。 DSP下位机会根据程序设置执行上位机设置的运动模式，只有在上位机重新致新的运动模式时才会改变。3.3 硬件系统的基本模块根据图9所示的硬件系统结构图，本节将分析几个重要模块。分为电源模块、无线通讯模块、指南针模块、液晶显示模块、DSP下位机模块、电机及驱动模块、全向轮。 DSP下位机在3.1节已经介绍过，这里不再赘述。完成毕业论文+按钮：（149-908-83-24）去掉中间的横线4 系统运动控制的MATLAB仿真本设计的全向运动控制系统采用三轴

16、全向控制，即控制三个电机的协调运动，实现车轮的全向运动。电机的控制对整个系统至关重要。本章主要分析了系统的PID控制和MATLAB仿真。4.1 电机 PID 控制电机是运动系统的执行器，无论DSP计算多么正确，但电机执行达不到要求，同一系统无法实现全向运动。为了使电机能够稳定、准确、快速地执行，PID是基于比例、积分和微分偏差的线性组合的反馈控制。是目前业界应用最广泛的控制方式。因此本设计采用PID分别对三台电机进行闭环控制。4.2 速度检测本设计选择增量 PID，即 4.1 节中描述的 PID。 PID 控制反馈是必不可少的。在本设计中，采用双编码器的Faulhaber减速电机，即增量式光电

17、编码器作为反馈环节。其工作原理如图14所示。光源照射的信号通过码盘和检测光栅，经光电检测装置检测，检测到的正弦值经转换电路转换成矩形波，最后输出A相、B相、Z相。 A相和B相相差90度，通过软件可以很好的获得此时电机的转速和方向。 AB相的获取是电机闭环控制的反馈量，关系到电机PID控制能否满足稳定性、精度和速度的要求。本设计采用输入捕捉CAP功能对脉冲进行采集，对采集量进行相应处理，得到PID控制的反馈。4.3 MATLAB 仿真在三轴控制系统中， PID是应用最广泛的控制算法。对于电机模型，这里设置电机模型，对该电机模型进行MATLAB仿真，观察PID控制算法的控制效果1011 。控制结果

18、对比曲线如图15所示，绿色曲线为期望值，蓝色曲线为PID控制跟踪曲线，红色曲线为误差曲线。实际速度可以通过编码器反馈。可见， PID控制方法的响应速度和跟踪精度均能满足系统要求。完成毕业论文+按钮：（149-908-83-24）去掉中间的横线5 系统软件设计根据前几章的模型建立分析和硬件系统搭建，本章主要介绍全向运动控制系统的编程。前两节介绍了下位机全向运动系统所使用的编程平台软件、整个系统的流程图和各部分子程序的流程图。最后一节介绍了上位机部分，编写了上位机的软件部分以及上位机下位机使用的通信协议。5.1 软件开发平台和模拟器本设计的核心控制芯片为TMS320LF2407A，其软件开发平台使

19、用官方工具CCSv3.3 ，仿真器使用XDS510-USB2.0。安装 CCS V3.3 后，会生成两个部分，分别是 CCStudio V3.3 和Setup CCStudio v3.3 。其中， Setup CCStudio v3.3用于配置开发工具的参考硬件，CCStudio V3.3用于建立项目编写软件。开发周期示意图如图 18 12所示。图 19 是 CCStudio V3.3 开发环境。5.2 运动控制软件设计由于本课题是基于DSP全向运动控制系统的软件设计，侧重于全向运动的控制，因此本设计中简化了检测部分，只需要采集罗盘和编码脉冲。其全向运动方式通过上位机无线传输指令，然后DSP接

20、收指令进行处理，最终控制电机的协调运动，并根据罗盘和编码器进行闭环控制，以达到最佳效果。6 实验验证与结果分析为了验证本设计中各种全向运动的准确性，本章将对各种全向运动形式进行测试和验证。设计测试场地，记录测试数据，分析结果。6.1 测试地点因为这种设计中使用的驱动轮是由尼龙制成的。驱动轮与地面接触部分的硬度比较大。如果驱动轮在普通地板上，摩擦力不够，就会出现严重的“打滑”现象。为了提供足够的摩擦力结合实验室条件，选用飞思卡尔赛道所用的KT板做一个120CM*100CM的测试场地，在场地上标出一个留有20cm边距的正方形，同时验证拐角的正确性。相隔45的虚线结构图如图26所示。6.2 各种性能

21、测试需要验证的测试有：PID验证、平移验证、原位旋转验证、边平移边旋转。设计和制造测试装置，分别测试各种性能。6.3影响因素分析影响系统性能的因素大部分来自外部，大致可以分为以下几种情况：(1)电池电量不足，导致系统电机启动或变化较大时电流不足。结果，系统不能很好地按照设定的形式移动。(2)外部磁场干扰，数字罗盘对磁场的感应非常灵敏，所以稍强的外部磁场会对系统产生干扰。一方面是金属铁，易被磁化，产生弱磁。在机房钢结构环境下，各个位置受到的磁干扰不同，经测试最大影响角可达30，严重影响系统性能。另一方面是电机。由于电动机具有强磁铁，因此产生磁场的磁场也会干扰指南针。去除干扰的方法只能通过硬磁补偿

22、减少一小部分干扰，大部分干扰无法去除。(3)主动轮干涉，本设计采用瑞典全向轮，车轮外侧有两排交错的侧轮，使主动轮在滚动过程中上下振动。同时，由于驱动轮的滚动，电机扭矩会发生变化。当 L=12 或 14.5cm 时，当两个外滑轮着地时，扭矩会发生 2.5cm 的变化，这会导致系统转动出现误差。完成毕业论文+按钮：（149-908-83-24）去掉中间的横线结束语全向机器人上的全向运动控制系统可以实现机器人在任意方向进行直线运动，而无需事先进行旋转运动。姿态，从而达到最终状态所需的姿态角。其特殊的移动方式使得全向机器人越来越受到重视和应用，其未来的实用领域和前景将更加广阔。本次设计完成的主要工作如

23、下：(1) 研究分析了目前几种运动结构，设计了互成120度的三轴运动结构。(2)对三轴运动控制结构进行数学建模，并进行运动学分析。详细分析计算了全向运动的几种运动形式。(3) 以TMS320LF2407A芯片为控制核心，采用其模块UART（SCI）、IIC（模拟）、PWM、I/O、CAP、外接扩展RAM。同时围绕核心外接设备编写和调试程序，达到了全方位控制的目的。(4)利用VC6.0的MSComm控件编写上位机软件，设计上位机的接口和功能，编写通信协议。这使得系统通过选择相关运动形式，修改上位机相关参数，实现运动系统的全方位运动。同时，上位机可显示系统当前状态及三个电机转速值。并通过游戏手柄实

24、现遥控系统的各种运动状态。(5) 实现了多台电机的数字增量PID控制，三台电机由各自的PID控制，互不影响，通过MATLAB进行仿真。计算出的速度可以很好地执行，提高了系统的精度。(6) 实现了全向运动控制系统的所有运动形式，并通过实验测试验证了其准确性。参考1 徐旭，石，叶真，曾祺。综述：RoboCup与第三届世界智能控制与自动化大会研究论文集M ，200 82 董斌，易建强，邓旭月。全向移动机器人结构与运动分析J机器人。 2003(25)3 海丹全方位移动平台的设计与控制D.：国防科技大学。 20094龚建伟. Visual C+/Turbo C 串行通信编程实践。电子工业, 20045

25、Raul Rojas：全方位控制，柏林自由大学，技术报告 B-10-03，FU。贝丁。 2003 年 6 月。6 和平 DSP原理与电机控制应用基于TMS320LF240X系列M.：航空航天大学，2008.7 王茂飞，程宇 TMS320C2000DSP技术与应用开发M.：清华大学，2007.8 Texas Instruments.TMS320LF/LC240 xA DSP Controllers Reference Guide-System and Peripherals SPRU357BM.December,2001.9 永华，一欣，卢生。新型PID控制及其应用M机械工业。 199810 薛

26、定宇控制系统的计算机辅助设计MATLAB语言与应用（第2版）M.：清华大学，2006.11 海丁基于Nios II的中型足球机器人底层控制系统的研究与实现D.大学。12 严波. 100个数字信号处理器-DSP应用实例M，：化工，200713 海丹全方位移动平台的设计与控制D.：国防科技大学，200614 洪林精通Visual C+串行通信技术与工程实践（第3版）.人民邮电，200815 龚建伟. Visual C+/Turbo C串口通信编程实战（第二版）M，：电子工业，2007.9附录基于DSP的新型运动控制系统软件清单如下：其中有GB161.H（存储液晶字体数据）、24C02.C（操作罗盘

27、）、design.C（运动控制策略）、ILI9163.C（液晶显示）、main.c（主程序）。因为程序比较大，所以本附录只包含了main.c和design.c两个文件。主要.c ：/* * */* 文件名：main.c */* 作者：李海清* /* 日期： 04/01/2011 */* 说明：该文件提供main函数，提供初始化 */* 串口致和中断接收程序，波特率设置为9600 */* IO口占用：PE1-PE6（PWM）PA3-PA6和PB0-PB7和PE7液晶屏使用*/* PF0=CAP5, PF1=cap6, PA3=cap1 三电机测速。 PE2=PWM8 PE4=PWM10, PE6

28、=PWM12 三电机*/* PC0-1 PF2-3 PF4-5 电机正反转 */* * */#include global.c#include math.h无效系统初始化（）；无效 Timer1Init();无效 ILI9163_init();无效踢狗（）；无效 PWM_Init();无效 IOinit();无效 Timer4Init();无效 Timer2Init();/* * */* 主程序 */* * */主要的（）系统初始化（）； /系统初始化IOinit(); /IO口初始化SCI_Init(); /SCI初始化PWM_Init(); /PWM初始化ILI9163_init(); /

29、液晶初始化Timer1Init(); /*定时器1初始化*/Timer4Init(); /*定时器4初始化*/Timer2Init(); /*定时器2初始化*/asm(CLRC INTM);显示（0 xff07,0 xffe0）； LCD_PutString(40,03,海清,0 x001f,0 x07e0); / LCD界面显示信息LCD_PutString(16,23,运动控制系统,0 x001f,0 x07e0); /LCD_PutString(0,143,指南针:,0 x001f,0 x07e0); /而（1）科学红（）； /判断和读取SCI数据速度PV（）； /速度函数/* */*

30、系统初始化*/* */无效系统初始化（）asm(SETC INTM); /* 禁用总中断 */asm(CLRC SXM); /* 禁用符号扩展 */asm(CLRC CNF); /* B0 块映射到片上 DARAM*/asm(CLRC OVM); /* 累加器结果正常溢出 */SCSR1=0 x834E； /* 系统时钟 CLKOUT=20*2=40M */ /* 开启ADC、EVA、EVB、CAN、SCI的时钟*/WDCR=0 x006F； /* 禁用看门狗，看门狗时钟除以64*/踢狗（）； /* 初始化看门狗*/IFR=0 xFFFF； /* 清除中断标志*/IMR=0 x000B； /*

31、 打开中断 1,2*/* */* IO口初始化 */* */无效 IOinit()MCRC=MCRC & 0 x0354; /PE0-PE7，PF0-PF5有效0000MCRA=MCRA & 0 x0008; /PA0-PA7,PB0-PB7有效0208MCRB=MCRB & 0 xFF00;PADATDIR=0 xF708； /PA口初始化值为0PBDATDIR=0 xFF00； /PB端口初始值为0佩达迪尔=0 xFF7F； /PE口初始值为0PFDATDIR=0 xFC33； /PF0-5为1个PF6输入PCDATDIR=0 xFFFF； /pc输出pc7-pc0 0101 0101/*

32、 */* PWM 初始化 */* */无效 PWM_Init()MCRC=MCRC | 0 x0054; /PE1-PE6为PWM端口7EEVBIFRA=0 xFFFF； / 清除中断标志ACTRB=0 x0666； /PWM12,10,8低有效，PWM 11,9,7高有效/DBCTONA=0 x0530; /启用死区定时器1，分频40M/16=2.5M，死区时间5*0.4us=2usT3PR=2500； /定时器3周期值，定时0.4us*2500=1ms /*2500CMPR4=0； /比较值CMPR5=0； /比较值CMPR6=0； /比较值COMCONB=0 xA600； /比较控制寄存

33、器T3CNT=0； /定时器3计数器清零EVBIMRA=0 x0080； /定时器3周期中断使能T3CON=0 x144E； /增加模式，TPS系数40M/16=2.5M，T3开启，void KickDog() /*踢看门狗*/WDKEY=0 x5555；WDKEY=0 xAAAA；设计.c：/* * */* 文件名：design.c */* 作者：李海清 */* 日期：2011 年 3 月 10 日 */*描述：该文件提供运动控制策略*/* * */#include GLOBAL.C#include math.h#define speed1 CMPR4 /宏定义speed1电机1速度#def

34、ine speed2 CMPR5 /宏定义speed1电机2速度#define speed3 CMPR6 /宏定义speed1电机2速度#define PVtime 3 /扫描时间，每个轮子检测PVtime ms#define Kp 0.8 /宏定义Kp#define Ki 0.2 /宏定义 Ki#define Kd 0.2 /宏定义Kd#define PI 3.14159 /宏定义PI无符号整数 flog=0,flogF=0,flogH=0,flogX=0,flogPID=0; /各种运动状态标志无符号整数 flogPV=0,pvlcd=0,flogPV1=0,flogPV2=0,flogP

35、V3=0;无符号整数 SCI_RXDATA,RXDINT14=0,0,0,0,0,0,0,0,0,0,0,0,0,0;int sum11=0 ,sum22=0 ,sum33=0;int sum11_0=0,sum22_0=0,sum33_0=0;int SCIn=0, i=0, SCISP1, SCISP2, SCISP3;int speed1_1=0,speed2_1=0,speed3_1=0,speed1_0=0,speed2_0=0,speed3_0=0;int tt5=0,tt6=0,tt1=0,t0=0,tt5_33=0,0,0,tt6_33=0,0,0,tt1_33=0 ,0,0;

36、无符号整数 tt55=0,tt66=0,tt11=0; /电机编码器值tt55 tt66 tt11分别为电机1-3的离散值无符号整数 scitt99=0,0,0,0,0,0,0,0,0;无符号整数 PVlb=0,CY_26_read=0,CY_26_Angle=0;无符号整数 SCIHspeed=0,SCIHangle=0,SCIHag_SN=0;无符号整数 SCIXspeed=0,SCIXangle=0;无符号整数 flogHStop=0,flogXSN=0;浮动 flogangle=0.0,angle11=0.0,angle22=0.0,angle33=0.0,angle=0.0;字符 S

37、CI_RXDATA114=0,0,0,0,0,0,0,0,0,0,0,0,0,0;/*SCI初始化*/无效 SCI_Init()MCRA=MCRA | 0 x0003; /IOPA0,IOPA1为串口SCICCR=0 x0007； /空闲多处理器模式，8个数据位，1个停止位，无奇偶校验位SCICTL1=0 x0003； /接收，致，外部时钟使能，SLEEP=1SCICTL2=0 x0002； /接收中断使能SCIPRI=0 x0000； /高中断优先级SCIHBAUD=0 x0002； /40M，波特率设置为9600SCILBAUD=0 x0008；SCICTL1=0 x0023； /串口初始

38、化完成/* 定时器 1 初始化 * * /无效 Timer1Init()T1PR=2500； /定时器1初始值，计时0.4us*2500=1msT1CNT=0；GPTCNA=0 x0000；EVAIMRA=EVAIMRA|0 x0080; / 定时器 1 周期中断使能EVAIFRA=EVAIFRA|0 xFFFF; / 清除中断标志T1CON=0 x144E； /增加模式，TPS系数40M/16=2.5M，T1开启/*定时器1-2初始化* * */无效 Timer2Init()T2PR=2500；T2CNT=0；WSGR=0 x0000；卡普纳=0 x2240；CAFIFOA=0 x0100；

39、EVAIMRC=EVBIMRC|0 x0001；EVAIFRC=EVBIFRC|0 xffff；T2CON=0 x1440； /1440/*定时器4初始化输入捕捉*/无效 Timer4Init()T4PR=2500；T4CNT=0；WSGR=0 x0000；CAPCONB=0 x3014；CAPFIFOB=0 x1500；EVBIMRC=EVBIMRC|0 x0006；EVBIFRC=EVBIFRC|0 xffff；T4CON=0 x1440； /1440/*SCI致*/void SCI_Send(unsigned data) /致0 x80-0 x87，共8字节数据SCITXBUF=数据；而

40、(SCICTL2 & 0 x0080)=0);/*SCI接收分解运动数据*/void SCIF() /执行速度分解程序，分别控制三个电机的速度无符号整数 i=1;for(i=1;i=360）HSDangle=HSDangle-360；HSD_SCIangle=HSDangle-CY_26_Angle； /设置角度与实际角度的差值absangle=abs(HSD_SCIangle); /设置实际间隙的绝对值如果（RXDINT10=1）如果（绝对角 180）if(HSD_SCIangle-358&HSD_SCIangle-180) CYspeed=15+(360-absangle)/5;否则 if

41、(HSD_SCIangle180) CYspeed=-15-(360-absangle)/5;否则 CYspeed=0;if(absangle=180) C y速度=15;别的 ; /防止如果（绝对角1&HSD_SCIangle180) CYspeed=15+absangle/5;否则 if(HSD_SCIangle-180) CYspeed=-15-absangle/5;否则 CYspeed=0;否则 CYspeed=0;CYangleSD=PI/2-flogangle；if(flogangle=0）PCDATDIR=PCDATDIR&0 xFFFE；否则 PCDATDIR=PCDATDIR

42、&0 xFFFD;如果（sum22_0=0）PFDATDIR=PFDATDIR&0 xFCFB；否则 PFDATDIR=PFDATDIR&0 xFCF7;如果（sum33_0=0）PFDATDIR=PFDATDIR&0 xFCEF；否则 PFDATDIR=PFDATDIR&0 xFCDF;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|0 xFFFF; flogPV1=0； 否

43、则 flogPV1=1;if(sum22=0) PFDATDIR=PFDATDIR|0 xFC0C; flogPV2=0； 否则 flogPV2=1;if(sum33=0) PFDATDIR=PFDATDIR|0 xFC30; flogPV3=0； 否则 flogPV3=1;如果（flog=1）LCD_Putnum(48,83,SCIHspeed,0 x001f,0 x07e0);LCD_Num(48,103,SCIHag_SN,0 x001f,0 x07e0);flogH=1； /合成平移运动标志，控制显示部分清屏，显示对应的运动模式和数据/*SCI接收旋转运动数据*/void SCIX()

44、 /执行旋转运动，控制旋转整数 SDangle=0；int SD_SCIangle=0,Xspeed=0,absangle=0;SD角度=SCIX角度+180；如果（SDangle=360）SDangle=SDangle-360；SD_SCIangle=SDangle-CY_26_Angle；absangle=abs(SD_SCIangle);如果（绝对角 180）if(SD_SCIangle-358&SD_SCIangle-180) Xspeed=15+( 360- absangle )/2;否则 if(SD_SCIangle180) Xspeed=-15-( 360- absangle )

45、/2;否则 Xspeed=0;如果（绝对角度=180）Xspeed=15；别的 ; /防止如果（绝对角1&SD_SCIangle180) Xspeed=15+absangle/2;否则 if(SD_SCIangle-180) Xspeed=-15-absangle/2;否则 Xspeed=0;sum11_0=X 速度；sum22_0=X 速度；sum33_0=X 速度；PCDATDIR=0 xFFFF;PFDATDIR=PFDATDIR|0 xFCFC;如果（sum11_0=0）PCDATDIR=PCDATDIR&0 xFFFE；否则 PCDATDIR=PCDATDIR&0 xFFFD;如果（

46、sum22_0=0）PFDATDIR=PFDATDIR&0 xFCFB；否则 PFDATDIR=PFDATDIR&0 xFCF7;如果（sum33_0=0）PFDATDIR=PFDATDIR&0 xFCEF；否则 PFDATDIR=PFDATDIR&0 xFCDF;sum11=abs(sum11_0);sum22=abs(sum22_0);sum33=abs(sum33_0);if(sum11=0) PCDATDIR=PCDATDIR|0 xFFFF; flogPV1=0； 否则 flogPV1=1;if(sum22=0) PFDATDIR=PFDATDIR|0 xFC0C; flogPV2=

47、0； 否则 flogPV2=1;if(sum33=0) PFDATDIR=PFDATDIR|0 xFC30; flogPV3=0； 否则 flogPV3=1;如果（flog=1）LCD_Putnum(48,83,SCIHspeed,0 x001f,0 x07e0);LCD_Num(48,103,SCIHag_SN,0 x001f,0 x07e0);flogX=1;/循环运动标志位，控制显示部分清屏，显示对应的运动模式和数据/*速度PID* * */int ee1=0,ee2=0,ee3=0,ee1_2=0,ee1_1=0,ee2_2=0,ee2_1=0,ee3_2=0,ee3_1=0;无效 P

48、IDPV()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 = 1; speed2_1 = 0; speed3_1 = 0; flogPID = 0; /speed1_1 = speed1_1 + Kp * (ee1-ee1_1) + Ki * ee1 + Kd * (ee1-2 * ee1_1 + ee1_2); / Kp Ki Kd

49、speed2_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)；如果（速度1_1 0）速度1_1 = 0；如果（速度2_1 0）速度2_1 = 0；如果（速度3_1 500）速度1_1=500；如果（速度2_1500）速度2_1=500；如果（速度3_1500）速度3_1=500；如果（flogPV1=1）速度1=速度1_1*5；如果（f

50、logPV2=1）速度2=速度2_1*5；如果（flogPV3=1）速度3=速度3_1*5；/* SCI阅读，判断动作形式* * */无效的 SciRed()无符号 i2=0;如果（SCI_RXDATA10=42&flog=1）if(SCI_RXDATA11=35) /合成翻译对于(i2=2;i213;i2+)RXDINTi2=SCI_RXDATA1i2-48； /处理接收到的SCI如果（flogH=0|flogF=1|flogX=1） /清除运动显示区域，开始(x, y Tuyu?x1, y1) 部分屏幕。开始 0,83，结束 128,143LCD_clear(0,63,128,64,0 x

51、ff07,0 xffe0);flogF=0； flogX=0；LCD_PutString(8,63,合成运动；平移,0 x001f,0 x07e0);LCD_PutString(0,83,速度:,0 x001f,0 x07e0);LCD_PutString(0,103,角度：,0 x001f,0 x07e0); 别的 ;SCIHspeed=RXDINT2*100+RXDINT3*10+RXDINT4； /翻译速度SCIHangle=RXDINT6*100+RXDINT7*10+RXDINT8；/平移角度，RXDINT5为顺时针和逆时针，1为顺时针，2为逆时针SCIHag_SN=RXDINT5*

52、1000+RXDINT6*100+RXDINT7*10+RXDINT8；flogHStop=RXDINT9；if(RXDINT5=1) flogangle=SCIHangle*2*PI/360;/变换角度值如180变换3.14否则 角度=360-SCIHangle; flogangle=角度*2*PI/360； /变换角度值如180变换3.14SCIH(); /合成运动子程序执行函数flog=0； /致接收标志位1接收，0不接收。在这里清零。else if(SCI_RXDATA11=64) / 旋转对于(i2=2;i213;i2+)RXDINTi2=SCI_RXDATA1i2-48;/处理接收到的SCI如果（flogX=0|flogF=1|flogH=1）LCD_clear(0,63,128,64,0 xff07,0 xffe0); LCD_PutString(8,63,旋转运动,0 x001f,0 x07e0);LCD_PutString(0,83,速度:,0 x001f,0 x07e0);LCD_PutString(0,103,角度：,0 x001f,0 x07e0);flogF=0； flogH=0；flogXSN=RXDINT2； /顺时针为1，逆时针为2SCIXspeed=RXDINT6*100+RXDINT7*10+RX