ABB机器人与埃尔森3D视觉通信Socket计算位置算法案例.txt

PROC TellRobCon()  //Socket通信
        SocketClose client_socket;
        SocketCreate client_socket;
        SocketConnect client_socket,"192.168.125.10",54600;  //定义IP地址与端口号
        IF workNum=0 THEN
            SocketSend client_socket\Str:="210";
            WaitTime 0.5;
            SocketSend client_socket\Str:="101";
        ENDIF
       workNum:=workNum+1;
        SocketReceive client_socket\Str:=receive_string\Time:=WAIT_MAX; //从远程服务器端接收数据
        part:=StrPart(receive_string,1,1); //从第一个字符起始截取1位字符
        !part1 := StrPart(receive_string,2,1);
        ok:=StrToVal(part,nval); // 字符转数值型

        IF ok THEN
            IF nval=0 THEN
                ClawPose:=CRobT(\Tool:=tGripRoll\WObj:=wobj0); 读取当前机器人位置与姿态信息
                send_string:="000";
                send_string:=send_string+","+NumToStr(ClawPose.trans.x,3)+","+NumToStr(ClawPose.trans.y,3)+","+NumToStr(ClawPose.trans.z,3)+","
                +NumToStr(ClawPose.rot.q1,6)+","+NumToStr(ClawPose.rot.q2,6)+","+NumToStr(ClawPose.rot.q3,6)+","+NumToStr(ClawPose.rot.q4,6);  //将XYZ位置坐标与四元数姿态转换为字符发送至3D视觉做处理
                SocketSend client_socket\Str:=send_string;
            ELSEIF nval=3 THEN
                !Extract X,Y.Z,A,B,C,D
                visionok:=TRUE;
                FOR it FROM 1 TO 7 DO
                    IF it=1 THEN
                        pose:=StrPart(receive_string,5,StrLen(receive_string)-5);
                    ELSE
                        pose:=StrPart(pose,found+1,StrLen(pose)-found);