MODULE Module1

VAR socketdev client_socket;

CONST robtarget PreAquisitionPoint:=[[165,90,-64.996],[1,0,0,0],[-
2,1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget WaitPoint:=[[-271.256,314.68,-64.996],[0.9008612,0,0,-
0.434107244],[-1,1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget PrePlacing:=[[175.1,146.761,-73.4],[0.707106781,0,0,-
0.707106781],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget PlacingPoint:=[[175.1,146.761,-18.4],[0.707106781,0,0,-
0.707106781],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget ReferencePointParts:=[[30,30,0],[1,0,0,0],[-
1,1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS wobjdata Workobject_Bases:=[FALSE,TRUE,"",[[736.5,-
79.561,21.6],[0,0,1,0]],[[0,0,0],[1,0,0,0]]];
PERS wobjdata Workobject_Parts:=[FALSE,TRUE,"",[[180,-
655,30.004],[0,0,1,0]],[[0,0,0],[1,0,0,0]]];

PROC main()
CONST num NUM_PARTS := 4;
VAR string str_data;
VAR string str_data_tmp;
VAR pos pos_recv{NUM_PARTS}; ! Store the 4 received positions, starting with the
largest part
VAR bool ok;
CONST NUM zP:=0;
SocketCreate client_socket;
SocketConnect client_socket, "127.0.0.1", 1024;

WHILE TRUE DO
! Expected string example:
! P1=(02.1,08.9,33.0),P2=(18.8,02.2,66.0),P3=(10.5,06.7,83.5),P4=(27.4,07.8,73.0)
! This string as a fixed size, with a total of 4*20 - 1 + 1 = 80 characters
! Receive the entire string
SocketReceive client_socket \Str := str_data \ReadNoOfBytes := 80 \Time :=
WAIT_MAX;
! Process and store each data point, and act accordingly

FOR i FROM 1 TO NUM_PARTS DO

! i*20+5 represents the index of the first digit for the ith part
! 14 is the number of characters between parenthesis
str_data_tmp := "[" + StrPart(str_data,(i-1)*20+5,14) + "]";
ok := StrToVal(str_data_tmp,pos_recv{i});

IF i=1 THEN

PlacingPath1:
 !ELSEIF i=2 THEN
! MoveJ WaitPoint,v600,z0,Gripper_TCP\WObj:=Workobject_Parts;
MoveL
PreAquisitionPoint,v600,z100,Gripper_TCP\WObj:=Workobject_Parts;
MoveL RelTool(ReferencePointParts,
pos_recv{i}.x*10,pos_recv{i}.y*10, 0
\Rz:=pos_recv{i}.z),v10,fine,Gripper_TCP\WObj:=Workobject_Parts;
SetDO doActivateGripper,1;
WaitDI diGripperClosed,1;
MoveL RelTool(ReferencePointParts,
pos_recv{i}.x*10,pos_recv{i}.y*10, -50
\Rz:=pos_recv{i}.z),v10,fine,Gripper_TCP\WObj:=Workobject_Parts;
PlacingPath2:
SetDO doActivateGripper,0;
WaitDI diGripperOpen,0;
MoveJ
PrePlacing,v100,z100,Gripper_TCP\WObj:=Workobject_Bases;
MoveJ WaitPoint,v100,z0,Gripper_TCP\WObj:=Workobject_Parts;
ELSEIF i=3 THEN
MoveJ WaitPoint,v600,z0,Gripper_TCP\WObj:=Workobject_Parts;
MoveL
PreAquisitionPoint,v600,z100,Gripper_TCP\WObj:=Workobject_Parts;
MoveL RelTool(ReferencePointParts,
pos_recv{i}.x*10,pos_recv{i}.y*10, 0
\Rz:=pos_recv{i}.z),v10,fine,Gripper_TCP\WObj:=Workobject_Parts;
SetDO doActivateGripper,1;
WaitDI diGripperClosed,1;
MoveL RelTool(ReferencePointParts,
pos_recv{i}.x*10,pos_recv{i}.y*10, -50
\Rz:=pos_recv{i}.z),v10,fine,Gripper_TCP\WObj:=Workobject_Parts;
PlacingPath3:
SetDO doActivateGripper,0;
WaitDI diGripperOpen,0;
MoveJ
PrePlacing,v100,z100,Gripper_TCP\WObj:=Workobject_Bases;
MoveJ WaitPoint,v100,z0,Gripper_TCP\WObj:=Workobject_Parts;
ELSEIF i=4 THEN
MoveJ WaitPoint,v600,z0,Gripper_TCP\WObj:=Workobject_Parts;
MoveL
PreAquisitionPoint,v600,z100,Gripper_TCP\WObj:=Workobject_Parts;
MoveL RelTool(ReferencePointParts,
pos_recv{i}.x*10,pos_recv{i}.y*10, 0
\Rz:=pos_recv{i}.z),v10,fine,Gripper_TCP\WObj:=Workobject_Parts;
SetDO doActivateGripper,1;
WaitDI diGripperClosed,1;
 MoveL RelTool(ReferencePointParts,
pos_recv{i}.x*10,pos_recv{i}.y*10, -50
\Rz:=pos_recv{i}.z),v10,fine,Gripper_TCP\WObj:=Workobject_Parts;
PlacingPath4:
SetDO doActivateGripper,0;
WaitDI diGripperOpen,0;
MoveJ
PrePlacing,v100,z100,Gripper_TCP\WObj:=Workobject_Bases;
MoveJ WaitPoint,v100,z0,Gripper_TCP\WObj:=Workobject_Parts;
ENDIF

ENDFOR

ENDWHILE

ENDPROC

PROC PlacingPath1()
MoveJ WaitPoint,v600,z0,Gripper_TCP\WObj:=Workobject_Parts;
MoveJ PrePlacing,v600,z0,Gripper_TCP\WObj:=Workobject_Bases;
MoveL PlacingPoint,v50,fine,Gripper_TCP\WObj:=Workobject_Bases;

ENDPROC

PROC PlacingPath2()
MoveJ WaitPoint,v600,z0,Gripper_TCP\WObj:=Workobject_Parts;
MoveJ PrePlacing,v600,z0,Gripper_TCP\WObj:=Workobject_Bases;
MoveL Offs(PlacingPoint,0,0,zP),v50,fine,Gripper_TCP\WObj:=Workobject_Bases;
ENDPROC

PROC PlacingPath3()
CONST NUM zP:=-20;
MoveJ WaitPoint,v600,z0,Gripper_TCP\WObj:=Workobject_Parts;
MoveJ PrePlacing,v600,z0,Gripper_TCP\WObj:=Workobject_Bases;
MoveL Offs(PlacingPoint,0,0,zP),v50,fine,Gripper_TCP\WObj:=Workobject_Bases;
ENDPROC

PROC PlacingPath4()
CONST NUM zP:=-30;
MoveJ WaitPoint,v600,z0,Gripper_TCP\WObj:=Workobject_Parts;
MoveJ PrePlacing,v600,z0,Gripper_TCP\WObj:=Workobject_Bases;
MoveL Offs(PlacingPoint,0,0,zP),v50,fine,Gripper_TCP\WObj:=Workobject_Bases;
ENDPROC
PROC PlacingPath()
 MoveJ WaitPoint,v600,z0,Gripper_TCP\WObj:=Workobject_Parts;
MoveJ PrePlacing,v600,z0,Gripper_TCP\WObj:=Workobject_Bases;
ENDPROC

ENDMODULE