对于不同的主控系统，机器⼈发送当前位置数据的⽅式也多种多样。如果使⽤PC机作为上位机来读取机器⼈实时位置信息，那么我们就可以通过使⽤IRC5 OPC Server来读取机器⼈位置数据，然后再发送给PC上位机；当然，也可以通过PC SDK对机器⼈控制器进⾏⼆次开发，然后通过PC Interface选项，直接读取控制器中机器⼈的位置信息。如果是使⽤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指令将以四元数形式表⽰的⾓度数据转换为欧拉⾓形式表⽰的旋转⾓度数据。完整的中断⼦程序代码如下所⽰。
TRAP iTrap!中断程序iTrap
!读取机器⼈当前位置，并将读取数据赋值给pCurrentPos
pCurrentPos:=CRobT();
!将读取到的机器⼈当前位置x、y、z坐标值分别赋值给变量x、y、z
x:=ans.x;
y:=ans.y;
z:=ans.z;
!将将读取到的机器⼈当前位置四元数⾓度值转换为欧拉⾓之后，分别赋值给变量rx、ry、rz
rx:=EulerZYX(\);
ry:=EulerZYX(\);
rz:=EulerZYX(\);
!调⽤发送机器⼈当前位置例⾏程序
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指令进⾏“逻辑或”运算，重新组成3
2位的单精度数据，并将其存放到声明好的dnum类型变量中。当然，这个移位操作，我们也可以在PLC中实现。
最后，使⽤setgo指令将转换好的机器⼈当前位置数据赋值到相应的组输出信号，然后发送给PLC。
PROC send_pCurrentPos()!发送机器⼈当前位置例⾏程序
!清空机器⼈位姿通⽤数据容器中间转换变量
ClearRawBytes rawbyte_x;
ClearRawBytes rawbyte_y;
ClearRawBytes rawbyte_z;
ClearRawBytes rawbyte_rx;
ClearRawBytes rawbyte_ry;
ClearRawBytes rawbyte_rz;
!将机器⼈当前位置数据按照float形式打包
PackRawBytes x,rawbyte_x,RawBytesLen(rawbyte_x)+1\Float4;
PackRawBytes y,rawbyte_y,RawBytesLen(rawbyte_y)+1\Float4;
PackRawBytes z,rawbyte_z,RawBytesLen(rawbyte_z)+1\Float4;
PackRawBytes rx,rawbyte_rx,RawBytesLen(rawbyte_rx)+1\Float4;
PackRawBytes ry,rawbyte_ry,RawBytesLen(rawbyte_ry)+1\Float4;
PackRawBytes rz,rawbyte_rz,RawBytesLen(rawbyte_rz)+1\Float4;
!将机器⼈位姿通⽤数据容器⾥的前4个字节数据分别保存到字节数组变量中
FOR i FROM 1 TO 4 DO
UnpackRawBytes rawbyte_x,i,byte_x{i}\Hex1;
UnpackRawBytes rawbyte_y,i,byte_y{i}\Hex1;
UnpackRawBytes rawbyte_z,i,byte_z{i}\Hex1;
UnpackRawBytes rawbyte_rx,i,byte_rx{i}\Hex1;
UnpackRawBytes rawbyte_ry,i,byte_ry{i}\Hex1;
UnpackRawBytes rawbyte_rz,i,byte_rz{i}\Hex1;
ENDFOR
!机器⼈数据格式转换（西门⼦PLC⾼低字节与机器⼈⾼低字节定义相反）
dn_x:=BitLShDnum(NumToDnum(byte_x{1}),24);!将单精度数据byte_x{1}转换为双精度类型后，左移24位，然后赋值给dn_x
dn_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}));
!使⽤相应的组输出信号，将机器⼈当前位置数据进⾏输出
setgo go_cx,dn_x;
setgo go_cy,dn_y;
setgo go_cz,dn_z;
setgo go_crx,dn_rx;
setgo go_cry,dn_ry;
setgo go_crz,dn_rz;
ENDPROC
机器⼈主程序编写
在主程序中编写定时器中断程序，中断时间间隔为0.5s，即每个0.5s读取⼀次机器⼈当前位置，并将当前位置数据发送给PLC。完整机器
⼈主程序代码如下所⽰。
运⾏测试李玟的胸
在PLC中进⾏组态编程，然后运⾏机器⼈，同时对PLC中相应的数据寄存器进⾏监视，可以看到机器⼈的实时位置数据已经发送过来了，并
且每隔0.5s刷新⼀次。西门⼦⽼版本的PLC实数数据的⾼低位字节顺序与ABB机器⼈的实数数据的⾼低位顺序是相反的；新版本的PLC，
⼩编还没有测试过，若是使⽤新版本的PLC，测试时可以注意⼀下。如果⾼低位字节顺序是⼀致的，那么只需要把发送机器⼈当前位置数据
⼦例⾏程序中的数据移位程序删除即可。对于三菱PLC、欧姆龙PLC或是其他品牌的PLC，读取机器⼈当前位置的程序都是类似的，感兴趣
的⼩伙伴可以⾃⼰测试⼀下。
附录：完成程序
MODULE Module1
CONST jointtarget jpos10:=[[0,0,0,0,30,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p10:=[[392.836308491,-156.6306903,309.536253784],[0.165037067,-0.000000128,0.986287365,0.000000003],[-1,0,-1,0],[9E+09,9E+09,9E CONST robtarget p20:=[[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, !声明机器⼈当前位姿变量
VAR robtarget pCurrentPos;
!声明机器⼈位姿存储变量
VAR num x;
VAR num y;
一年级语文上册期末试卷VAR num z;
VAR num rx;
VAR num ry;
VAR num rz;
!声明机器⼈位姿通⽤数据容器中间转换变量
VAR rawbytes rawbyte_x;
VAR rawbytes rawbyte_y;于正质疑tfboys新海报抄袭
VAR rawbytes rawbyte_z;
VAR rawbytes rawbyte_rx;
VAR rawbytes rawbyte_ry;
VAR rawbytes rawbyte_rz;
!声明机器⼈位姿字节型中间数据转换变量
VAR byte byte_x{4};
VAR byte byte_y{4};
VAR byte byte_z{4};
VAR byte byte_rx{4};
VAR byte byte_ry{4};
VAR byte byte_rz{4};
!声明双精度类型机器⼈位姿数据变量
VAR dnum dn_x;
VAR dnum dn_y;
VAR dnum dn_z;
VAR dnum dn_rx;
VAR dnum dn_ry;
VAR dnum dn_rz;
!声明中断数据变量
VAR intnum TrapNum;
PROC main()
IDelete TrapNum;!取消识别号为TrapNum的中断
CONNECT TrapNum WITH iTrap;!将识别号为TrapNum的中断与中断程序iTrap连接
ITimer 0.5,TrapNum;!循环调⽤中断程序TrapNum，循环周期为0.5s
!循环执⾏机器⼈运动程序
WHILE TRUE DO
MoveAbsJ jpos10\NoEOffs,v50,z50,tool0;
MoveJ p10,v50,z50,tool0;
MoveJ p20,v50,z50,tool0;
MoveAbsJ jpos10\NoEOffs,v50,z50,tool0;
ENDWHILE
ENDPROC
TRAP iTrap!中断程序iTrap
TRAP iTrap!中断程序iTrap
!读取机器⼈当前位置，并将读取数据赋值给pCurrentPos
pCurrentPos:=CRobT();
!将读取到的机器⼈当前位置x、y、z坐标值分别赋值给变量x、y、z
x:=ans.x;
y:=ans.y;
z:=ans.z;
!将将读取到的机器⼈当前位置四元数⾓度值转换为欧拉⾓之后，分别赋值给变量rx、ry、rz
rx:=EulerZYX(\);
ry:=EulerZYX(\);
rz:=EulerZYX(\);
!调⽤发送机器⼈当前位置例⾏程序
send_pCurrentPos;
ENDTRAP
PROC send_pCurrentPos()!发送机器⼈当前位置例⾏程序
!清空机器⼈位姿通⽤数据容器中间转换变量
ClearRawBytes rawbyte_x;
ClearRawBytes rawbyte_y;
ClearRawBytes rawbyte_z;
ClearRawBytes rawbyte_rx;
ClearRawBytes rawbyte_ry;
ClearRawBytes rawbyte_rz;
!将机器⼈当前位置数据按照float形式打包
PackRawBytes x,rawbyte_x,RawBytesLen(rawbyte_x)+1\Float4;
PackRawBytes y,rawbyte_y,RawBytesLen(rawbyte_y)+1\Float4;
PackRawBytes z,rawbyte_z,RawBytesLen(rawbyte_z)+1\Float4;
PackRawBytes rx,rawbyte_rx,RawBytesLen(rawbyte_rx)+1\Float4;
PackRawBytes ry,rawbyte_ry,RawBytesLen(rawbyte_ry)+1\Float4;
PackRawBytes rz,rawbyte_rz,RawBytesLen(rawbyte_rz)+1\Float4;
!将机器⼈位姿通⽤数据容器⾥的前4个字节数据分别保存到字节数组变量中
FOR i FROM 1 TO 4 DO
UnpackRawBytes rawbyte_x,i,byte_x{i}\Hex1;
UnpackRawBytes rawbyte_y,i,byte_y{i}\Hex1;
UnpackRawBytes rawbyte_z,i,byte_z{i}\Hex1;
UnpackRawBytes rawbyte_rx,i,byte_rx{i}\Hex1;
UnpackRawBytes rawbyte_ry,i,byte_ry{i}\Hex1;
UnpackRawBytes rawbyte_rz,i,byte_rz{i}\Hex1;
奔跑吧兄弟改名ENDFOR
!
机器⼈数据格式转换（西门⼦PLC⾼低字节与机器⼈⾼低字节定义相反）
dn_x:=BitLShDnum(NumToDnum(byte_x{1}),24);!将单精度数据byte_x{1}转换为双精度类型后，左移24位，然后赋值给dn_x
dn_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));
发布评论