ABB机器人发送实时位置数据

对于不同的主控系统，机器人发送当前位置数据的方式也多种多样。如果使用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:=pCurrentPos.trans.x;
        y:=pCurrentPos.trans.y;
        z:=pCurrentPos.trans.z;
        !将将读取到的机器人当前位置四元数角度值转换为欧拉角之后，分别赋值给变量rx、ry、rz
        rx:=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。

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+09,9E+09,9E+09,9E+09]];
    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,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;
    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
        !读取机器人当前位置，并将读取数据赋值给pCurrentPos
        pCurrentPos:=CRobT();
        !将读取到的机器人当前位置x、y、z坐标值分别赋值给变量x、y、z
        x:=pCurrentPos.trans.x;
        y:=pCurrentPos.trans.y;
        z:=pCurrentPos.trans.z;
        !将将读取到的机器人当前位置四元数角度值转换为欧拉角之后，分别赋值给变量rx、ry、rz
        rx:=EulerZYX(\x,pCurrentPos.rot);
        ry:=EulerZYX(\y,pCurrentPos.rot);
        rz:=EulerZYX(\z,pCurrentPos.rot);
        !调用发送机器人当前位置例行程序
        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));
        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
ENDMODULE