MODULE MainModule

PERS robtarget pHome:=[[1518.35057503,0,877.822906024],[0,0,1,0],[0,0,0,0],

[9E9,9E9,9E9,9E9,9E9,9E9]];
PERS robtarget pPick:=[[1518.360445356,-11.456071507,523.888125175],
[0,0.000000009,1,0],[-1,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
PERS robtarget pPlaceBase:=[[-285.531972806,1859.243453208,99.429150272],
[0,0.000000358,1,0],[1,0,1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
PERS robtarget pPlace:=[[414.468,1359.24,99.4292],[0,0.707107,0.707107,0],
[1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget pActualPos:=[[1505,0,877.823],[1.81E-06,0,-1,0],[0,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS bool bPalletFull:=FALSE;
PERS num nCount:=6;
PERS robtarget pPlaceBase_1:=[[1505.00,0.00,1276.00],[1.81E-06,0,-1,0],
[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

PROC main()
rInitAll;
WHILE TRUE DO
IF bPalletFull = FALSE THEN
rPick;
rPlace;
ELSE
WaitTime 0.3;
ENDIF
ENDWHILE
ENDPROC

PROC rInitAll()
pActualPos:=CRobT(\tool:=tGripper);
pActualPos.trans.z:=pHome.trans.z;
MoveL pActualPos, v500, fine, tGripper\WObj:=wobj0;
MoveJ pHome,v500,fine,tGripper\WObj:=wobj0;
bPalletFull:=FALSE;
nCount:=1;
Reset doGrip;
ENDPROC
PROC rPick()
MoveJ Offs(pPick,0,0,300),v2000,z50,tGripper\WObj:=wobj0;
WaitDI diBoxInPos, 1;
MoveL pPick,v500,fine,tGripper\WObj:=wobj0;
Set doGrip;
WaitTime 1;
MoveL Offs(pPick,0,0,300),v500,z50,tGripper\WObj:=wobj0;
ENDPROC
PROC rPlace()
rPosition;
MoveJ Offs(pPlace,0,0,300),v2000,z50,tGripper\WObj:=wobj0;
MoveL pPlace,v500,fine,tGripper\WObj:=wobj0;
Reset doGrip;
WaitTime 1;
MoveL Offs(pPlace,0,0,300),v500,z50,tGripper\WObj:=wobj0;
rPlaceRD;
ENDPROC
PROC rPlaceRD()
Incr nCount;
IF nCount>=41 THEN
nCount:=1;
bPalletFull:=TRUE;
MoveJ pHome,v1000,fine,tGripper\WObj:=wobj0;
ENDIF
ENDPROC
PROC rPosition()
TEST nCount
CASE 1:
pPlace:=RelTool(pPlaceBase,0,0,0\Rz:=0);
CASE 2:
pPlace:=RelTool(pPlaceBase_1,0,0,0\Rz:=0);
CASE 3:
pPlace:=RelTool(pPlaceBase,-600,0,0\Rz:=0);
CASE 4:
pPlace:=RelTool(pPlaceBase_1,-600,0,0\Rz:=0);
CASE 5:
pPlace:=RelTool(pPlaceBase,100,-500,0\Rz:=90);
CASE 6:
pPlace:=RelTool(pPlaceBase_1,100,-500,0\Rz:=90);
CASE 7:
pPlace:=RelTool(pPlaceBase,-300,-500,0\Rz:=90);
CASE 8:
pPlace:=RelTool(pPlaceBase_1,-300,-500,0\Rz:=90);
CASE 9:
pPlace:=RelTool(pPlaceBase,-700,-500,0\Rz:=90);
CASE 10:
pPlace:=RelTool(pPlaceBase_1,-700,-500,0\Rz:=90);
CASE 11:
pPlace:=RelTool(pPlaceBase,100,-100,-250\Rz:=90);
CASE 12:
pPlace:=RelTool(pPlaceBase_1,100,-100,-250\Rz:=90);
CASE 13:
pPlace:=RelTool(pPlaceBase,-300,-100,-250\Rz:=90);
CASE 14:
pPlace:=RelTool(pPlaceBase_1,-300,-100,-250\Rz:=90);
CASE 15:
pPlace:=RelTool(pPlaceBase,-700,-100,-250\Rz:=90);
CASE 16:
pPlace:=RelTool(pPlaceBase_1,-700,-100,-250\Rz:=90);
CASE 17:
pPlace:=RelTool(pPlaceBase,0,-600,-250\Rz:=0);
CASE 18:
pPlace:=RelTool(pPlaceBase_1,0,-600,-250\Rz:=0);
CASE 19:
pPlace:=RelTool(pPlaceBase,-600,-600,-250\Rz:=0);
CASE 20:
pPlace:=RelTool(pPlaceBase_1,-600,-600,-250\Rz:=0);
CASE 21:
pPlace:=RelTool(pPlaceBase,0,0,-500\Rz:=0);
CASE 22:
pPlace:=RelTool(pPlaceBase_1,0,0,-500\Rz:=0);
CASE 23:
pPlace:=RelTool(pPlaceBase,-600,0,-500\Rz:=0);
CASE 24:
pPlace:=RelTool(pPlaceBase_1,-600,0,-500\Rz:=0);
CASE 25:
pPlace:=RelTool(pPlaceBase,100,-500,-500\Rz:=90);
CASE 26:
pPlace:=RelTool(pPlaceBase_1,100,-500,-500\Rz:=90);
CASE 27:
pPlace:=RelTool(pPlaceBase,-300,-500,-500\Rz:=90);
CASE 28:
pPlace:=RelTool(pPlaceBase_1,-300,-500,-500\Rz:=90);
CASE 29:
pPlace:=RelTool(pPlaceBase,-700,-500,-500\Rz:=90);
CASE 30:
pPlace:=RelTool(pPlaceBase_1,-700,-500,-500\Rz:=90);
CASE 31:
pPlace:=RelTool(pPlaceBase,100,-100,-750\Rz:=90);
CASE 32:
pPlace:=RelTool(pPlaceBase_1,100,-100,-750\Rz:=90);
CASE 33:
pPlace:=RelTool(pPlaceBase,-300,-100,-750\Rz:=90);
CASE 34:
pPlace:=RelTool(pPlaceBase_1,-300,-100,-750\Rz:=90);
 CASE 35:
pPlace:=RelTool(pPlaceBase,-700,-100,-750\Rz:=90);
CASE 36:
pPlace:=RelTool(pPlaceBase_1,-700,-100,-750\Rz:=90);
CASE 37:
pPlace:=RelTool(pPlaceBase,0,-600,-750\Rz:=0);
CASE 38:
pPlace:=RelTool(pPlaceBase_1,0,-600,-750\Rz:=0);
CASE 39:
pPlace:=RelTool(pPlaceBase,-600,-600,-750\Rz:=0);
CASE 40:
pPlace:=RelTool(pPlaceBase_1,-600,-600,-750\Rz:=0);
DEFAULT:
Stop;
ENDTEST
ENDPROC
PROC rModify()
MoveL pHome,v1000,fine,tGripper\WObj:=wobj0;
MoveL pPick,v1000,fine,tGripper\WObj:=wobj0;
MoveL pPlaceBase,v1000,fine,tGripper\WObj:=wobj0;
MoveL pPlaceBase_1,v1000,fine,tGripper\WObj:=wobj0;
ENDPROC
ENDMODULE