MODULE Module1
VAR intnum x1:=0;
VAR byte byte1{7};
VAR socketdev plc;
CONST robtarget p10:=[[300.29,43.32,572.52],[0.0112584,0.00376771,-0.948239,-0.317336],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p20:=[[50.18,-32.92,700.20],[0.436243,0.482279,0.759345,0.0222028],[-1,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p30:=[[22.77,-332.46,597.48],[0.0155268,0.497229,0.746593,-0.441726],[-1,-1,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p40:=[[-32.16,-36.73,740.79],[0.395355,0.16397,-0.702491,0.568608],[-1,-1,-2,4],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p50:=[[204.94,144.39,410.86],[0.121204,0.954404,-0.0286826,0.271294],[0,-1,-2,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST jointtarget jpos10:=[[-79.4939,-39.2035,0.00108763,33.5104,89.7457,-0.011057],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PROC main()
tx;
dkzd;
WHILE TRUE DO
movej p10,v100,z0,tool0;
movej p20,v100,z0, tool0;
movej p30,v100,z0, tool0;
movej p40,v300,z0, tool0;
movej p50,v300,z0, tool0;
ENDWHILE
ENDPROC
PROC tx()
socketclose plc;
socketcreate plc;
waittime 0.2;
socketconnect plc,"192.168.101.5",2000;
waittime 0.2;
ENDPROC
PROC dkzd()
IDelete x1;
CONNECT x1 WITH zd;
ITimer 0.2,x1;
endproc
TRAP zd
GetJointData 1\Speed:=byte1{1};
GetJointData 2\Speed:=byte1{2};
GetJointData 3\Speed:=byte1{3};
GetJointData 4\Speed:=byte1{4};
GetJointData 5\Speed:=byte1{5};
GetJointData 6\Speed:=byte1{6};!35-40行获取当前角速度单位是°/s
byte1{1}:=abs(round(byte1{1}));
byte1{2}:=abs(round(byte1{2}));
byte1{3}:=abs(round(byte1{3}));
byte1{4}:=abs(round(byte1{4}));
byte1{5}:=abs(round(byte1{5}));
byte1{6}:=abs(round(byte1{6}));!41-46行把获取的角速度去小数,变正
byte1{7}:=round(sudu*1000);!将当前线速度化成mm/s并去小数
SocketSend plc\Data:=byte1;
ENDTRAP
ENDMODULE