IRB-120机械臂socket通信接受上位机指令运行程序段
IRB-120机械臂使用socket通信接受上位机指令程序段
功能：可接受坐标系坐标和单轴坐标，可实现相对运动或绝对运动
可与上位机握手避免连接错误
可向上位机发送当前位置（坐标系、单轴）
MODULE Module1
CONST 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;
ENDIF
move0:
!对接收到的报文进行解释,找出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" THEN
WaitTime 0.5;
ELSE
TPWrite "Failed.";
GOTO end;
ENDIF
TPWrite "OK. Now sending position.";
!发送坐标报文
SocketSend socket1\Str:= now_Position;
!显示发送成功,然后循环回到等待接收报文部分
TPWrite "Success.";
ENDWHILE
!结束程序段,跳到此则跳出循环,并结束程序.
end:
EXIT;
ENDPROC
ENDMODULE
浙公网安备 33010602011771号