ABB 机械臂的部分代码

abb端socket通讯代码：

MODULE connect_pc
    TASK PERS  tooldata tool100:=[TRUE,[[0,0,0],[0,0.70711,0,0.70711]],[1.3,[0,0,150],[1,0,0,0],0,0,0]];
  VAR robtarget P100:=[[1000,0,500],[1,0,0,0],[-1,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];

  VAR num posedatab{7};
  VAR bool okb;
  VAR pos pos1b;
  VAR num Eangle{3};
  VAR string strtrans;
  VAR string strrot;
  VAR string str;
  VAR string PC_adressb;
  !192.168.125.1//127.0.0.1;
  !虚拟机地址127.0.0.1，真实机械臂地址为192.168.125.1
  VAR string IRC5_adressb:="192.168.125.1"; 
  CONST num pro_portb :=8885;
  VAR socketdev server_socketb;
  VAR socketdev client_socketb;
  VAR socketstatus statusb;
  VAR num peek_valueb:=65;
  VAR num p:=5;
  VAR pose objectb;
  !error
  VAR string str_datab;
  VAR string str_databb:="[1,1000.1,0.2,300.3,0,90,0]";
  VAR num retry_nob:=0;
  
PROC main3()
  	intialb;
  	!MoveAbsJ [[0,0,0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]]\NoEOffs,v300,z50,tool0;
  	WHILE TRUE DO
  		str_datab:="[1,1000.1,0.2,300.03,0,90,0]";
  		TRAN;
  		SocketSend client_socketb\str:=str;!发送位姿，第一次发送的是P100初始值
  		Receivedatab;!接受数据，第一位为校验位，后六位为位姿
  		Moveitb;
  	ENDWHILE
  	socketclose server_socketb;!关闭
  	socketclose client_socketb;
  ENDPROC
 
  PROC Moveitb()!??
  	IF posedatab{1}=1 OR posedatab{1}=2 THEN
  		MoveJ P100,v50,fine,tool0;
  	ELSE
  		MoveL Offs(P100,200,0,0),v20,fine,tool100;
  	ENDIF
  ENDPROC
 
  PROC TRAN()
  	Eangle{1}:=EulerZYX(\X,P100.rot);!姿态四元数转欧拉角
  	Eangle{2}:=EulerZYX(\Y,P100.rot);
  	Eangle{3}:=EulerZYX(\Z,P100.rot);
  	strrot:=ValToStr(Eangle);
  	strtrans:=ValToStr(P100.trans);!位值类型转string
  	str:=strtrans+strrot;!字节合并
  ENDPROC
 
  PROC Receivedatab()
  	SocketReceive client_socketb\Str:=str_datab\Time:=200;!接收字符串
  	TPWrite"inputvlaue="+str_datab;!示教器上显示
  	okb:=strtoval(str_datab,posedatab);!字符串转数字变量
  	IF okb=TRUE THEN
  		 P100.trans.