Fanuc机器人与Cognex In
一、硬件介绍
机器人型号：FANUCRobotR-1000iA
相机型号：Cognexinsight2000系列智能相机
通讯协议：Ethernet/IP
//说明：本文采用的通讯方式为：FANUC机器人与Cognex相机直接通讯，该方法是否适用于其他型号的FANUC机器人后续会继续验证。通讯成功后，相机向机械手发送数据，该数据存储在机器人的组GI[*]中。同时，机器人通过GO[*]向相机发送数据。
硬件要求：
1、相机：PC端安装CognexIn-SightExplorer软件。
该软件可从康耐视官网免费下载，下载链接：https://support.cognex.com/zh-cn/downloads/detail/in-sight/3839/1033
2、机器人：需购买1A05B-2600-R784和1A05B-2600-R785协议
二、软件配置方法
1、相机：相机上电后，打开In-SightExplorer软件，在“传感器设置”中按照如下方法配置：
配置完成后保存。
2、机器人：
①检查机器人是否开放R784和R785协议。
检查方法：
②配置机器人Ethernet/IP:
通过示教器，MENU→I/O→Ethernet/IP，按照如下方式配置：
说明：
1、描述可以任意填写。
2、机器人默认类型为ADP，需要使用示教器切换为SCN。如果机器人没有开放R785协议，则无法切换。
3、启用默认为无效，需改为有效。
4、状态分为离线、待定和运行中三种，默认状态为离线，待全部配置完成后重新启动机器人，启动方式选择冷启动，状态会自动变为运行中。
③机器人输入输出设置
机器人从GI[9]开始接受相机发送的数据，数据长度16位，GI[1]到GI[8]代表的含义待确定。
GI[]分配如下：
一览界面：
机器人向相机发送数据，从GO[8]开始，长度8位。
GO[]配置如下：
机器人I/O信号分配：
DO[145]:触发使能，该信号常亮。
DO[146]：触发拍照
DO[147]：数据使能
注：触发拍照时，DO[146]置ON，需等待0.5s后，再置OFF。否则会出现机器人收不到数据的情况。
智能机器人广告宣传语
我们主要从事智能制造、智能机械行业，品牌名称“小工匠”，产品为机器手臂、机器人，具有智能控制的特点，特征集一句智能机器人广告宣传语，要求作品新颖、简单易懂，有记忆点，朗朗上口，突出核心理念――小帮手，要求体现小工匠是人们的小帮手，具有灵活、高效、智能、贴心等特点。
经过举办征集活动，小编特选摘了部分精彩应征作品予以展示，以供品鉴（仅限参考）：
1、小工匠，为你上天入地，做你的左膀右臂。
2、赢在制造，我有小工匠。
3、是机器人，更是家人。
4、小工匠有一手，制造业好帮手。
5、小工匠机器人――小帮手，太给力。
6、智慧小工匠，给力小帮手。
7、我们有工匠精神，还有工匠。
8、妙在智能贵在高效――小工匠。
9、小工匠，智能制造领导者。
10、高科技核芯，小工匠引擎。
11、创新无止境，奋斗不停留。
12、有了小工匠，干活更高效。
13、小工匠有大智慧――小工匠。
14、智能小帮手，高效劳动力。
15、贴心的爱，智能的手。
16、我有一颗智慧的“芯”――小工匠。
17、高效小工匠，智能全方位。
18、小工匠，服务智造每一天。
19、给小工匠一个机会还你一个奇迹。
20、科技小工匠，生活智慧帮。
21、智能制造第一步，小工匠带你找出路。
22、匠芯智控生产，小工匠。
23、智能全方位，创新零距离。
24、智造高手――小工匠。
25、小工匠相助，智能高效快人一步。
26、机器人万千，小工匠领先。
27、智能小工匠，科技小帮手。
28、好快多干，（不拿工资的）劳动模范。
29、小工匠，真心帮你。
30、创领世界，智赢未来。
31、智慧的小工匠，知心的好帮手。
32、智能高效生产的倡导者――小工匠。
33、智赢世界，领跑未来。
34、小工匠，专门解决您的“智造”难题。
35、有了小工匠，效益成倍长。
36、小工匠――与众不同的大帮手。
37、智控小工匠，领跑中国造。
38、与小工匠携手，制造业得心应手。
39、笑谈智造，赢造未来。
40、小工匠机器人――小工匠，太能干。
41、每一次服务，都是好平（评）+――小工匠。
42、用小工匠，读懂中国制造。
43、小帮手，wWw.jiNTang114.ORg工能匠心。
44、科技无限，智赢未来。
45、小工匠，助你工成圆满。
46、不厌其烦的为您打下手――小工匠。
47、身手不凡――小工匠。
48、贴心小工匠，智慧“取”天下。
49、智我能，更轻松――小工匠。
50、小工匠――家庭好帮手。
51、精准服务，高效贴心。
52、小工匠――模仿人类的智能手臂。
53、携手小工匠，生产不用慌。
54、小帮手有大智慧――小工匠。
55、智能誉九州，生活好帮手。
56、小工匠，技能有点强。
57、小工匠，新智慧，新格局。
58、小工匠机器人――小工匠，大作用。
59、小工匠――中国智能机械先行者。
机器人视觉识别技术简介
基于颜色特征的物体识别系统对于不同颜色的分别提取和识别
(以上两幅图片由某大学机器人实验室负责人暨机器人天空主编LiuWeichao友情提供)
随着计算机科学和自动控制技术的发展，越来越多的不同种类的智能机器人出现在生产生活中，视觉系统作为智能机器人系统中一个重要的子系统，也越来越受到人们的重视。
视觉系统是一个非常复杂的系统，它既要做到图像的准确采集还要做到对外界变化反应的实时性，同时还需要对外界运动的目标进行实时跟踪。因此，视觉系统对硬件和软件系统都提出了较高的要求。目前比较流行的足球机器人技术，它的视觉系统属于比较典型的快速识别和反应类型。
机器视觉系统是指用计算机来实现人的视觉功能，也就是用计算机来实现对客观的三维世界的识别。人类视觉系统的感受部分是视网膜，它是一个三维采样系统。三维物体的可见部分投影到网膜上，人们按照投影到视网膜上的二维的像来对该物体进行三维理解(对被观察对象的形状、尺寸、离开观察点的距离、质地和运动特征等的理解)。
机器视觉系统的输入装置可以是摄像机、转鼓等，它们都把三维的影像作为输入源，即输入计算机的就是三维管观世界的二维投影。如果把三维客观世界到二维投影像看作是一种正变换的话，则机器视觉系统所要做的是从这种二维投影图像到三维客观世界的逆变换，也就是根据这种二维投影图像去重建三维的客观世界。
机器视觉系统主要由三部分组成：图像的获取、图像的处理和分析、输出或显示。图像的获取实际上是将被测物体的可视化图像和内在特征转换成能被计算机处理的一系列数据，它主要由三部分组成：照明，图像聚焦形成，图像确定和形成摄像机输出信号。视觉信息的处理技术主要依赖于图像处理方法，它包括图像增强、数据编码和传输、平滑、边缘锐化、分割、特征抽取、图像识别与理解等内容。经过这些处理后，输出图像的质量得到相当程度的改善，既改善了图像的视觉效果，又便于计算机对图像进行分析、处理和识别。
机器人视觉系统主要是利用颜色、形状等信息来识别环境目标。以机器人对颜色的识别为例：当摄像头获得彩色图像以后，机器人上的嵌入计算机系统将模拟视频信号数字化，将像素根据颜色分成两部分：感兴趣的像素(搜索的目标颜色)和不感兴趣的像素(背景颜色)。然后，对这些感兴趣的像素进行RGB颜色分量的匹配。为了减少环境光强度的影响，可把RGB颜色域空间转化到HIS颜色空间。
在足球机器人的彩色视觉系统中，程序是根据贴在机器人小车顶上的色标来识别机器人是属于哪一队，以及是几号队员的。由于在每个机器人小车顶上有两种颜色的色标，分别是队标和队员标。因此，识别工作的第一步是把图像中的每一个像素，根据颜色分类到一组离散的色彩类中。
颜色分类常用的方法有线性色彩阈值法、最近邻域法和阈值向量法等。
其中，线性色彩阈值法是用线性平面把色彩空间分割开来，其阈值的确定可采用直接取阈值和通过自动训练来获取目标颜色范围等方法，也可以采用神经网络和多参数决策树方法来进行自学习，以获得合适的阈值;而用最近邻域分类法分割图像时，则利用隶属度函数，即根据最大的隶属度来判断这个颜色属于哪个类;阈值向量法是先使用一组事先确定的阈值向量来把色彩值在色彩空间中的位置来判断其属于哪种颜色。
在色彩分类之后，必须对各个颜色类的点进行处理，最终辨识出场上的各个敌我队员和球在场上的位置和方向角。识别时，通常的做法是对分类后的像素进行一次扫描，即将相邻的同种颜色的像素连成色块。
基于阈值向量的颜色识别
一、色彩空间选择
对于采用基于彩色图像分割的方法识别目标时，首先要选择合适的颜色空间，常用的颜色空间有RGB、YUV、HSV、CMY等。颜色空间的选择直接影响到图像分割和目标识别的效果。
RGB——最常用的颜色空间，其中亮度等于R、G、B3个分量之和。RGB颜色空间是不均匀的颜色空间，两个颜色之间的知觉差异与空间中两点间的欧氏距离不成线性比例，而且R、G、B值之间的相关性很高，对同一颜色属性，在不同条件(光源种类、强度和物体反射特性)下，RGB值很分散，对于识别某种特定颜色，很难确定其阈值和其在颜色空间中的分布范围。因此通常会选择能从中分离出亮度分量的颜色空间，其中最常见的是YUV和HSV颜色空间。
HSV——接近人眼感知色彩的方式，H为色调(Hue)，S为色饱和度(Saturation)，V为亮度(Value)。色调H能准确地反映颜色种类，对外界光照条件变化敏感度低，但是H和S均为R、G、B的非线性变换，存在奇异点，在奇异点附近即使R、G、B的值有很小变化也引起变换值有很大的跳动。
YUV——RGB颜色空间线性变化为亮度色彩空间。是为了解决彩色电视机与黑白电视机的兼容问题而提出的。Y表示亮度(Luminance)，UV用来表示色差(Chrominance)。YUV表示法的重要性是它的亮度信号(Y)和色度信号(U、V)是相互独立的。所谓色差是指基色信号中的3个分量信号(即R、G、B)与亮度信号之差。
YUV格式与RGB存在如下关系：
二、阈值确定和色彩判断
在确定阈值时，首先通过采集样本进行训练，从而得到预定的几种颜色在YUV空间的分量的上下阈值，如图2所示。
当一个待判定的像素在色彩空间中的位置落在这个长方体中时，就认为该像素属于要找的颜色，从而完成对图像颜色的识别。在Y空间中，Y值表示亮度，因它的变化很大，所以只考虑了U和V的值，在进行颜色判断时，首先分别建立U、V的阈值向量。
在颜色识别后进行图像分割，在图像分割中采用了种子填充算法，其整个种子的填充是和像素点的颜色同时进行的，一开始不是对所有的像素进行处理，而是分块进行的。当中心点是所要识别的颜色时，就以这个点为种子向四周扩散，并判定周围像素点的颜色，直到填满整个块。
ABB机器人发送实时位置数据
对于不同的主控系统，机器人发送当前位置数据的方式也多种多样。如果使用PC机作为上位机来读取机器人实时位置信息，那么我们就可以通过使用IRC5OPCServer来读取机器人位置数据，然后再发送给PC上位机；当然，也可以通过PCSDK对机器人控制器进行二次开发，然后通过PCInterface选项，直接读取控制器中机器人的位置信息。如果是使用PLC作为上位机来读取机器人实时位置信息，那么我们就可以通过工业现场通信，如ProfiBus、ProfiNet、DeviceNet等，然后使用ABB机器人内置的数据处理指令编写实时位置数据发送程序，来实现机器人位置数据的发送。
变量声明机器人的当前位置就是工具末端的当前位置，也就是TCP的当前坐标，它是由x、y、z的坐标值以及分别绕x、y、z轴旋转的角度值组成，这些数据的类型均是实数类型。在ABB机器人中，使用num与dnum来表示实数，其中num类型与西门子PLC中的real类型一致，都是32位的单精度实数；而dnum类型数据是64位的双精度实数。因此，在机器人中，我们可以声明num类型变量来存放机器人的当前位置数据。同时，声明其他类型的数据变量，作为数据处理的中间转换变量。变量声明代码如下所示。
组输出信号配置ABB机器人传输实数数据的方式大致可以分为两种：一、使用模拟量输出信号传输实数数据，由于模拟量信号自身抗干扰性能差，并且需要加装价格昂贵的模量信号扩展模块，因此，在传输大量的实数数据的场合中，一般很少使用模拟量信号；二、使用组输出信号传输实数数据，组输出信号不仅可以通过加装价格相对低廉的数字量I/O信号扩展模块实现，也可以通过加装现场通信模块的方式实现。本例，使用第二种方式，通过ProfiBus现场总线通信的形式来传输机器人当前位置数据。
由于机器人当前位置数据都是32位的单精度实数类型，所以，我们定义的每一个组输出信号长度也应该是32位。
中断子程序编写创建中断子例行程序，并在其中编写读取机器人当前位置x、y、z坐标数据程序。由于ABB机器人系统中使用四元数形式表示TCP绕相应坐标轴的旋转角度，因此，这里需要使用EulerZYX指令将以四元数形式表示的角度数据转换为欧拉角形式表示的旋转角度数据。完整的中断子程序代码如下所示。
TRAPiTrap!中断程序iTrap!读取机器人当前位置，并将读取数据赋值给pCurrentPospCurrentPos:=CRobT();!将读取到的机器人当前位置x、y、z坐标值分别赋值给变量x、y、zx:=pCurrentPos.trans.x;y:=pCurrentPos.trans.y;z:=pCurrentPos.trans.z;!将将读取到的机器人当前位置四元数角度值转换为欧拉角之后，分别赋值给变量rx、ry、rzrx:=EulerZYX(x,pCurrentPos.rot);ry:=EulerZYX(y,pCurrentPos.rot);rz:=EulerZYX(z,pCurrentPos.rot);!调用发送机器人当前位置例行程序send_pCurrentPos;ENDTRAP机器人当前位置数据发送子程序编写创建发送机器人当前位置数据子例行程序，并将其在中断子程序中调用。首先，将读取的机器人当前位置数据使用PackRawBytes指令按照Float形式进行打包，然后将其存放到已经声明的rawbyte类型容器变量的连续四个字节中。
然后使用FOR指令进行循环解包操作，即将打包好的数据使用UnpackRawBytes指令解包到声明的byte类型四维数组变量中，每一个字节对应数组变量中的一维byte元素。
由于西门子PLC中数据采用大端存储模式，而ABB机器人中的数据采用的是小端存储模式，为了发送的数据能够被PLC正确解析，因此，我们需要对机器人的位置数据进行移位操作。所谓的大端模式（Big-endian），是指数据的高字节，保存在内存的低地址中，而数据的低字节，保存在内存的高地址中，地址由小向大增加，而数据从高位往低位放；小端模式（Little-endian）是指数据的高字节保存在内存的高地址中，而数据的低字节保存在内存的低地址中，这种存储模式将地址的高低和数据位权值有效结合起来，高地址部分权值高，低地址部分权值低，和我们的逻辑方法一致。
对于ABB机器人当前位置数据的移位操作：首先使用NumToDnum指令将解包后的byte类型数据转换为dnum类型，然后按照西门子PLC中实数类型数据高低字节顺序进行移位操作，数据移位指令使用BitLShDnum。同时，将四个移位后的byte类型数据使用BitOrDnum指令进行“逻辑或”运算，重新组成32位的单精度数据，并将其存放到声明好的dnum类型变量中。当然，这个移位操作，我们也可以在PLC中实现。
最后，使用setgo指令将转换好的机器人当前位置数据赋值到相应的组输出信号，然后发送给PLC。
PROCsend_pCurrentPos()!发送机器人当前位置例行程序!清空机器人位姿通用数据容器中间转换变量ClearRawBytesrawbyte_x;ClearRawBytesrawbyte_y;ClearRawBytesrawbyte_z;ClearRawBytesrawbyte_rx;ClearRawBytesrawbyte_ry;ClearRawBytesrawbyte_rz;!将机器人当前位置数据按照float形式打包PackRawBytesx,rawbyte_x,RawBytesLen(rawbyte_x)+1Float4;PackRawBytesy,rawbyte_y,RawBytesLen(rawbyte_y)+1Float4;PackRawBytesz,rawbyte_z,RawBytesLen(rawbyte_z)+1Float4;PackRawBytesrx,rawbyte_rx,RawBytesLen(rawbyte_rx)+1Float4;PackRawBytesry,rawbyte_ry,RawBytesLen(rawbyte_ry)+1Float4;PackRawBytesrz,rawbyte_rz,RawBytesLen(rawbyte_rz)+1Float4;!将机器人位姿通用数据容器里的前4个字节数据分别保存到字节数组变量中FORiFROM1TO4DOUnpackRawBytesrawbyte_x,i,byte_x{i}Hex1;UnpackRawBytesrawbyte_y,i,byte_y{i}Hex1;UnpackRawBytesrawbyte_z,i,byte_z{i}Hex1;UnpackRawBytesrawbyte_rx,i,byte_rx{i}Hex1;UnpackRawBytesrawbyte_ry,i,byte_ry{i}Hex1;UnpackRawBytesrawbyte_rz,i,byte_rz{i}Hex1;ENDFOR!机器人数据格式转换（西门子PLC高低字节与机器人高低字节定义相反）dn_x:=BitLShDnum(NumToDnum(byte_x{1}),24);!将单精度数据byte_x{1}转换为双精度类型后，左移24位，然后赋值给dn_xdn_x:=BitOrDnum(dn_x,BitLShDnum(NumToDnum(byte_x{2}),16));!将单精度数据byte_x{2}转换为双精度类型后，左移16位，并与dn_x做或运算，然后赋值给自己dn_x:=BitOrDnum(dn_x,BitLShDnum(NumToDnum(byte_x{3}),8));!将单精度数据byte_x{2}转换为双精度类型后，左移8位，并与dn_x做或运算，然后赋值给自己dn_x:=BitOrDnum(dn_x,NumToDnum(byte_x{4}));!将单精度数据byte_x{2}转换为双精度类型后，与dn_x做或运算，然后赋值给自己!机器人数据格式转换dn_y:=BitLShDnum(NumToDnum(byte_y{1}),24);dn_y:=BitOrDnum(dn_y,BitLShDnum(NumToDnum(byte_y{2}),16));dn_y:=BitOrDnum(dn_y,BitLShDnum(NumToDnum(byte_y{3}),8));dn_y:=BitOrDnum(dn_y,NumToDnum(byte_y{4}));!机器人数据格式转换dn_z:=BitLShDnum(NumToDnum(byte_z{1}),24);dn_z:=BitOrDnum(dn_z,BitLShDnum(NumToDnum(byte_z{2}),16));dn_z:=BitOrDnum(dn_z,BitLShDnum(NumToDnum(byte_z{3}),8));dn_z:=BitOrDnum(dn_z,NumToDnum(byte_z{4}));!机器人数据格式转换dn_rx:=BitLShDnum(NumToDnum(byte_rx{1}),24);dn_rx:=BitOrDnum(dn_rx,BitLShDnum(NumToDnum(byte_rx{2}),16));dn_rx:=BitOrDnum(dn_rx,BitLShDnum(NumToDnum(byte_rx{3}),8));dn_rx:=BitOrDnum(dn_rx,NumToDnum(byte_rx{4}));!机器人数据格式转换dn_ry:=BitLShDnum(NumToDnum(byte_ry{1}),24);dn_ry:=BitOrDnum(dn_ry,BitLShDnum(NumToDnum(byte_ry{2}),16));dn_ry:=BitOrDnum(dn_ry,BitLShDnum(NumToDnum(byte_ry{3}),8));dn_ry:=BitOrDnum(dn_ry,NumToDnum(byte_ry{4}));!机器人数据格式转换dn_rz:=BitLShDnum(NumToDnum(byte_rz{1}),24);dn_rz:=BitOrDnum(dn_rz,BitLShDnum(NumToDnum(byte_rz{2}),16));dn_rz:=BitOrDnum(dn_rz,BitLShDnum(NumToDnum(byte_rz{3}),8));dn_rz:=BitOrDnum(dn_rz,NumToDnum(byte_rz{4}));!使用相应的组输出信号，将机器人当前位置数据进行输出setgogo_cx,dn_x;setgogo_cy,dn_y;setgogo_cz,dn_z;setgogo_crx,dn_rx;setgogo_cry,dn_ry;setgogo_crz,dn_rz;ENDPROC机器人主程序编写在主程序中编写定时器中断程序，中断时间间隔为0.5s，即每个0.5s读取一次机器人当前位置，并将当前位置数据发送给PLC。完整机器人主程序代码如下所示。
运行测试在PLC中进行组态编程，然后运行机器人，同时对PLC中相应的数据寄存器进行监视，可以看到机器人的实时位置数据已经发送过来了，并且每隔0.5s刷新一次。西门子老版本的PLC实数数据的高低位字节顺序与ABB机器人的实数数据的高低位顺序是相反的；新版本的PLC，小编还没有测试过，若是使用新版本的PLC，测试时可以注意一下。如果高低位字节顺序是一致的，那么只需要把发送机器人当前位置数据子例行程序中的数据移位程序删除即可。对于三菱PLC、欧姆龙PLC或是其他品牌的PLC，读取机器人当前位置的程序都是类似的，感兴趣的小伙伴可以自己测试一下。
附录：完成程序
MODULEModule1CONSTjointtargetjpos10:=[[0,0,0,0,30,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONSTrobtargetp10:=[[392.836308491,-156.6306903,309.536253784],[0.165037067,-0.000000128,0.986287365,0.000000003],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONSTrobtargetp20:=[[392.836306583,149.260106132,309.53614795],[0.165037087,-0.000000142,0.986287362,0],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];!声明机器人当前位姿变量VARrobtargetpCurrentPos;!声明机器人位姿存储变量VARnumx;VARnumy;VARnumz;VARnumrx;VARnumry;VARnumrz;!声明机器人位姿通用数据容器中间转换变量VARrawbytesrawbyte_x;VARrawbytesrawbyte_y;VARrawbytesrawbyte_z;VARrawbytesrawbyte_rx;VARrawbytesrawbyte_ry;VARrawbytesrawbyte_rz;!声明机器人位姿字节型中间数据转换变量VARbytebyte_x{4};VARbytebyte_y{4};VARbytebyte_z{4};VARbytebyte_rx{4};VARbytebyte_ry{4};VARbytebyte_rz{4};!声明双精度类型机器人位姿数据变量VARdnumdn_x;VARdnumdn_y;VARdnumdn_z;VARdnumdn_rx;VARdnumdn_ry;VARdnumdn_rz;!声明中断数据变量VARintnumTrapNum;PROCmain()IDeleteTrapNum;!取消识别号为TrapNum的中断CONNECTTrapNumWITHiTrap;!将识别号为TrapNum的中断与中断程序iTrap连接ITimer0.5,TrapNum;!循环调用中断程序TrapNum，循环周期为0.5s!循环执行机器人运动程序WHILETRUEDOMoveAbsJjpos10NoEOffs,v50,z50,tool0;MoveJp10,v50,z50,tool0;MoveJp20,v50,z50,tool0;MoveAbsJjpos10NoEOffs,v50,z50,tool0;ENDWHILEENDPROCTRAPiTrap!中断程序iTrap!读取机器人当前位置，并将读取数据赋值给pCurrentPospCurrentPos:=CRobT();!将读取到的机器人当前位置x、y、z坐标值分别赋值给变量x、y、zx:=pCurrentPos.trans.x;y:=pCurrentPos.trans.y;z:=pCurrentPos.trans.z;!将将读取到的机器人当前位置四元数角度值转换为欧拉角之后，分别赋值给变量rx、ry、rzrx:=EulerZYX(x,pCurrentPos.rot);ry:=EulerZYX(y,pCurrentPos.rot);rz:=EulerZYX(z,pCurrentPos.rot);!调用发送机器人当前位置例行程序send_pCurrentPos;ENDTRAPPROCsend_pCurrentPos()!发送机器人当前位置例行程序!清空机器人位姿通用数据容器中间转换变量ClearRawBytesrawbyte_x;ClearRawBytesrawbyte_y;ClearRawBytesrawbyte_z;ClearRawBytesrawbyte_rx;ClearRawBytesrawbyte_ry;ClearRawBytesrawbyte_rz;!将机器人当前位置数据按照float形式打包PackRawBytesx,rawbyte_x,RawBytesLen(rawbyte_x)+1Float4;PackRawBytesy,rawbyte_y,RawBytesLen(rawbyte_y)+1Float4;PackRawBytesz,rawbyte_z,RawBytesLen(rawbyte_z)+1Float4;PackRawBytesrx,rawbyte_rx,RawBytesLen(rawbyte_rx)+1Float4;PackRawBytesry,rawbyte_ry,RawBytesLen(rawbyte_ry)+1Float4;PackRawBytesrz,rawbyte_rz,RawBytesLen(rawbyte_rz)+1Float4;!将机器人位姿通用数据容器里的前4个字节数据分别保存到字节数组变量中FORiFROM1TO4DOUnpackRawBytesrawbyte_x,i,byte_x{i}Hex1;UnpackRawBytesrawbyte_y,i,byte_y{i}Hex1;UnpackRawBytesrawbyte_z,i,byte_z{i}Hex1;UnpackRawBytesrawbyte_rx,i,byte_rx{i}Hex1;UnpackRawBytesrawbyte_ry,i,byte_ry{i}Hex1;UnpackRawBytesrawbyte_rz,i,byte_rz{i}Hex1;ENDFOR!机器人数据格式转换（西门子PLC高低字节与机器人高低字节定义相反）dn_x:=BitLShDnum(NumToDnum(byte_x{1}),24);!将单精度数据byte_x{1}转换为双精度类型后，左移24位，然后赋值给dn_xdn_x:=BitOrDnum(dn_x,BitLShDnum(NumToDnum(byte_x{2}),16));!将单精度数据byte_x{2}转换为双精度类型后，左移16位，并与dn_x做或运算，然后赋值给自己dn_x:=BitOrDnum(dn_x,BitLShDnum(NumToDnum(byte_x{3}),8));!将单精度数据byte_x{2}转换为双精度类型后，左移8位，并与dn_x做或运算，然后赋值给自己dn_x:=BitOrDnum(dn_x,NumToDnum(byte_x{4}));!将单精度数据byte_x{2}转换为双精度类型后，与dn_x做或运算，然后赋值给自己!机器人数据格式转换dn_y:=BitLShDnum(NumToDnum(byte_y{1}),24);dn_y:=BitOrDnum(dn_y,BitLShDnum(NumToDnum(byte_y{2}),16));dn_y:=BitOrDnum(dn_y,BitLShDnum(NumToDnum(byte_y{3}),8));dn_y:=BitOrDnum(dn_y,NumToDnum(byte_y{4}));!机器人数据格式转换dn_z:=BitLShDnum(NumToDnum(byte_z{1}),24);dn_z:=BitOrDnum(dn_z,BitLShDnum(NumToDnum(byte_z{2}),16));dn_z:=BitOrDnum(dn_z,BitLShDnum(NumToDnum(byte_z{3}),8));dn_z:=BitOrDnum(dn_z,NumToDnum(byte_z{4}));!机器人数据格式转换dn_rx:=BitLShDnum(NumToDnum(byte_rx{1}),24);dn_rx:=BitOrDnum(dn_rx,BitLShDnum(NumToDnum(byte_rx{2}),16));dn_rx:=BitOrDnum(dn_rx,BitLShDnum(NumToDnum(byte_rx{3}),8));dn_rx:=BitOrDnum(dn_rx,NumToDnum(byte_rx{4}));!机器人数据格式转换dn_ry:=BitLShDnum(NumToDnum(byte_ry{1}),24);dn_ry:=BitOrDnum(dn_ry,BitLShDnum(NumToDnum(byte_ry{2}),16));dn_ry:=BitOrDnum(dn_ry,BitLShDnum(NumToDnum(byte_ry{3}),8));dn_ry:=BitOrDnum(dn_ry,NumToDnum(byte_ry{4}));!机器人数据格式转换dn_rz:=BitLShDnum(NumToDnum(byte_rz{1}),24);dn_rz:=BitOrDnum(dn_rz,BitLShDnum(NumToDnum(byte_rz{2}),16));dn_rz:=BitOrDnum(dn_rz,BitLShDnum(NumToDnum(byte_rz{3}),8));dn_rz:=BitOrDnum(dn_rz,NumToDnum(byte_rz{4}));!使用相应的组输出信号，将机器人当前位置数据进行输出setgogo_cx,dn_x;setgogo_cy,dn_y;setgogo_cz,dn_z;setgogo_crx,dn_rx;setgogo_cry,dn_ry;setgogo_crz,dn_rz;ENDPROCENDMODULE