IRB-120机械臂使用socket通信接受上位机指令程序段
功能：可接受坐标系坐标和单轴坐标，可实现相对运动或绝对运动
可与上位机握手避免连接错误
可向上位机发送当前位置（坐标系、单轴）
MODULE Module1CONST robtarget phome:=[[364.353852323,0,594.000007845],[0.499999961,0,0.866025426,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST jointtarget jpos10:=[[-9,-14,24,9,77,-15],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];TASK PERS tooldata tool1:=[TRUE,[[0,0,0],[1,0,0,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];TASK PERS wobjdata wobj1:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];CONST robtarget Target_10:=[[329.569435314,226.251705913,408.944462824],[0.676573537,-0.495214417,0.518157305,0.168890315],[0,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];PROC main()VAR robtarget pCurrentPos;VAR socketdev socket1;VAR string temp;VAR num startBit_x;VAR num endBit_x;VAR num lenBit_x;VAR num startBit_y;VAR num endBit_y;VAR num lenBit_y;VAR num startBit_z;VAR num endBit_z;VAR num lenBit_z;VAR num endBit;VAR string length;VAR string strx;VAR string stry;VAR string strz;VAR num x;VAR num y;VAR num z;VAR bool ok;VAR num current_x;VAR num current_y;VAR num current_z;VAR string str_x;VAR string str_y;VAR string str_z;VAR string now_Position;VAR num startBit_1;VAR num endBit_1;VAR num lenBit_1;VAR num startBit_2;VAR num endBit_2;VAR num lenBit_2;VAR num startBit_3;VAR num endBit_3;VAR num lenBit_3;VAR num startBit_4;VAR num endBit_4;VAR num lenBit_4;VAR num startBit_5;VAR num endBit_5;VAR num lenBit_5;VAR num startBit_6;VAR num endBit_6;VAR num lenBit_6;VAR string str_1;VAR string str_2;VAR string str_3;VAR string str_4;VAR string str_5;VAR string str_6;VAR string sendstr_1;VAR string sendstr_2;VAR string sendstr_3;VAR string sendstr_4;VAR string sendstr_5;VAR string sendstr_6;VAR num num_1;VAR num num_2;VAR num num_3;VAR num num_4;VAR num num_5;VAR num num_6;VAR jointtarget currentPos;VAR jointtarget targetPos;!初始化,先回原点MoveAbsJ jpos10\NoEOffs,v1000,fine,tool1;TPErase;!先关闭，防止上次没有正确关闭SocketClose socket1;SocketCreate socket1;SocketConnect socket1,"192.168.125.2", 1026;!连接上之后显示一条日志,已连接TPWrite "Connected.";!发送握手包SocketSend socket1\Str:="T";!TPWrite "Handshaking...";!等待接收,并且把接受到的东西显示.!SocketReceive socket1\Str:=temp;!TPWrite "Received:" + temp;!判断,如果返回报文正确,则进行下一步,否则退出程序.!IF temp = "T" THEN! TPWrite "Success.";! GOTO run;!ELSE! TPWrite "Failed.";! GOTO end;!ENDIF!run:!死循环WHILE TRUE DO!等待接收格式报文TPWrite "Waiting...";SocketReceive socket1\Str:=temp;TPWrite "Received:" + temp;!收到格式报文后,返回报文T,表示已经收到格式报文,上位机可以发送坐标报文SocketSend socket1\Str:="T";!判断格式报文的内容,并决定进行下一步操作.IF temp = "0" THEN!为0时,为坐标系运动,进行坐标系操作.TPWrite "MOVEL";SocketReceive socket1\Str:=temp;TPWrite "Received:" + temp;GOTO move0;ELSEIF temp = "1" THEN!为1时,为单轴运动,进行单轴操作.TPWrite "MOVEJ";SocketReceive socket1\Str:=temp;TPWrite "Received:" + temp;GOTO move1;ENDIFmove0:!对接收到的报文进行解释,找出xyz字母在字符串中的位置startBit_x:=1;endBit_x:=StrFind(temp,startBit_x,"x");lenBit_x:=endBit_x-startBit_x;startBit_y:=endBit_x+1;endBit_y:=StrFind(temp,startBit_y,"y");lenBit_y:=endBit_y-startBit_y;startBit_z:=endBit_y+1;endBit_z:=StrFind(temp,startBit_z,"z");lenBit_z:=endBit_z-startBit_z;!根据上述程序运行结果拆出xyz字符串坐标strx:=StrPart(temp,startBit_x,lenBit_x);stry:=StrPart(temp,startBit_y,lenBit_y);strz:=StrPart(temp,startBit_z,lenBit_z);!将xyz字符串坐标转为数值ok:=StrToVal(strx,x);ok:=StrToVal(stry,y);ok:=StrToVal(stry,z);!显示目标坐标值.判断解释是否正确TPWrite "Now moving to: "+ strx + " X " + stry + " Y " + strz + " Z";!移动,使用相对坐标移动.MoveL Offs(CRobT(),x,y,z), v500, fine, tool1\WObj:=wobj1;!转到向上位机发送坐标的程序段GOTO sendposition;move1:!对接收报文进行解释!接收报文为6个坐标值,其中以英文逗号作为分隔startBit_1:=1;endBit_1:=StrFind(temp,startBit_1,",");lenBit_1:=endBit_1-startBit_1;startBit_2:=endBit_1+1;endBit_2:=StrFind(temp,startBit_2,",");lenBit_2:=endBit_2-startBit_2;startBit_3:=endBit_2+1;endBit_3:=StrFind(temp,startBit_3,",");lenBit_3:=endBit_3-startBit_3;startBit_4:=endBit_3+1;endBit_4:=StrFind(temp,startBit_4,",");lenBit_4:=endBit_4-startBit_4;startBit_5:=endBit_4+1;endBit_5:=StrFind(temp,startBit_5,",");lenBit_5:=endBit_5-startBit_5;startBit_6:=endBit_5+1;endBit_6:=StrFind(temp,startBit_6,",");lenBit_6:=endBit_6-startBit_6;!按照以上分隔结果将字符串拆解str_1:=StrPart(temp,startBit_1,lenBit_1);str_2:=StrPart(temp,startBit_2,lenBit_2);str_3:=StrPart(temp,startBit_3,lenBit_3);str_4:=StrPart(temp,startBit_4,lenBit_4);str_5:=StrPart(temp,startBit_5,lenBit_5);str_6:=StrPart(temp,startBit_6,lenBit_6);!将字符串格式的坐标转为数值格式坐标ok:=StrToVal(str_1,num_1);ok:=StrToVal(str_2,num_2);ok:=StrToVal(str_3,num_3);ok:=StrToVal(str_4,num_4);ok:=StrToVal(str_5,num_5);ok:=StrToVal(str_6,num_6);!显示解释后的坐标,验证解释是否正确TPWrite "Now moving to:"+ str_1 + "," + str_2 + "," + str_3 + "," + str_4 + "," + str_5 + "," + str_6;!将解释后的坐标赋值给目标坐标变量targetPos.robax.rax_1 := num_1;targetPos.robax.rax_2 := num_2;targetPos.robax.rax_3 := num_3;targetPos.robax.rax_4 := num_4;targetPos.robax.rax_5 := num_5;targetPos.robax.rax_6 := num_6;!移动到坐标MoveAbsJ targetPos, v100, fine, tool0;GOTO sendposition;sendposition:!发送坐标程序段!获取机器人当前坐标currentPos := CJointT();!由于上位机接收格式为字符串格式,将数值转为字符串sendstr_1 := NumToStr(currentPos.robax.rax_1,3);sendstr_2 := NumToStr(currentPos.robax.rax_2,3);sendstr_3 := NumToStr(currentPos.robax.rax_3,3);sendstr_4 := NumToStr(currentPos.robax.rax_4,3);sendstr_5 := NumToStr(currentPos.robax.rax_5,3);sendstr_6 := NumToStr(currentPos.robax.rax_6,3);!获取当前坐标系坐标pCurrentPos := CRobT();current_x := pCurrentPos.trans.x;current_y := pCurrentPos.trans.y;current_z := pCurrentPos.trans.z;!将数值坐标转为字符串坐标str_x := NumToStr(pCurrentPos.trans.x,3);str_y := NumToStr(pCurrentPos.trans.y,3);str_z := NumToStr(pCurrentPos.trans.z,3);!当前坐标now_Position := str_x + "x" + str_y + "y" + str_z + "z" + sendstr_1 + "," + sendstr_2 + "," + sendstr_3 + "," + sendstr_4 + "," + sendstr_5 + "," + sendstr_6 + ".";!显示当前坐标TPWrite "Now position:" + now_Position;!判断当前字符串长度endBit:=Strlen(now_Position);!长度转为字符串形式length:=NumToStr(endBit,0);!返回坐标值字符串长度显示TPWrite "Next length:" + length;!发送报文,预告下一条坐标报文的长度!由于上位机需要指定接收报文的长度，故需预告坐标报文长度SocketSend socket1\Str:= length;SocketReceive socket1\Str:=temp;TPWrite "Received:" + temp;!判断对方返回报文.返回报文为T则表示对方做好准备接受下一条坐标报文IF temp = "T" THENWaitTime 0.5;ELSETPWrite "Failed.";GOTO end;ENDIFTPWrite "OK. Now sending position.";!发送坐标报文SocketSend socket1\Str:= now_Position;!显示发送成功,然后循环回到等待接收报文部分TPWrite "Success.";ENDWHILE!结束程序段,跳到此则跳出循环,并结束程序.end:EXIT;ENDPROC
ENDMODULE