MODULE Module1
CONST robtarget pHome:=[[1405.50,0.00,1620.00],[2.35481E-8,-7.53893E-9,1,-1.77528E-16],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget pLPick:=[[1600.00,-1100.00,740.00],[1.01751E-7,-1.31278E-7,1,-7.4175E-8],[-1,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget pRPlace:=[[1600.00,1100.00,340.00],[9.86443E-9,-1.70914E-8,1,-1.23181E-8],[0,-1,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget pLPick90:=[[1400.00,-700.00,740.00],[1.50459E-7,0.707107,0.707107,-6.22807E-8],[-1,-1,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget pRPlace90:=[[1400.00,700.00,340.00],[8.97325E-9,0.707107,0.707107,4.92759E-9],[0,-1,1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS robtarget pLPickSafety:=[[800,-700,640],[1.50459E-7,0.707107,0.707107,-6.22807E-8],[-1,-1,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS robtarget pRPlaceSafety:=[[800,700,1040],[8.97325E-9,0.707107,0.707107,4.92759E-9],[0,-1,1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST num L:=600;
CONST num W:=200;
CONST num H:=200;
CONST num LHNum:=6;
CONST num LVNum:=2;
CONST num Layer:=3;
PROC main()
MoveJ pHome, v1500, fine, tVacuume;
Reset doVacuume;
FOR i FROM 1 TO (LHNum+LVNum)*Layer DO
IF i<=LHNum THEN
pLPickSafety:=Offs(Offs(pLPick,-W*(i-1),0,-H*(Layer-3)),0,0,300);
MoveJ pLPickSafety, v1500, z50, tVacuume;
MoveL Offs(pLPick,-W*(i-1),0,-H*(Layer-3)), v1000, fine, tVacuume;
Set doVacuume;
WaitDI diVacuumeOK,1;
MoveL pLPickSafety, v1000, fine, tVacuume;
pRPlaceSafety:=Offs(Offs(pRPlace,-W*(i-1),0,H*(Layer-3)),0,0,300);
MoveJ pRPlaceSafety, v1500, z50, tVacuume;
MoveL Offs(pRPlace,-W*(i-1),0,H*(Layer-3)), v1000, fine, tVacuume;
Reset doVacuume;
WaitDI diVacuumeOK,0;
MoveL pRPlaceSafety, v1000, fine, tVacuume;
ELSEIF i>LHNum AND i<=(LHNum+LVNum) THEN
pLPickSafety:=Offs(Offs(pLPick90,-L*(i-LHNum-1),0,-H*(Layer-3)),0,0,300);
MoveJ pLPickSafety, v1500, z50, tVacuume;
MoveL Offs(pLPick90,-L*(i-LHNum-1),0,-H*(Layer-3)), v1000, fine, tVacuume;
Set doVacuume;
WaitDI diVacuumeOK,1;
MoveL pLPickSafety, v1000, fine, tVacuume;
pRPlaceSafety:=Offs(Offs(pRPlace90,-L*(i-LHNum-1),0,H*(Layer-3)),0,0,300);
MoveJ pRPlaceSafety, v1500, z50, tVacuume;
MoveL Offs(pRPlace90,-L*(i-LHNum-1),0,H*(Layer-3)), v1000, fine, tVacuume;
Reset doVacuume;
WaitDI diVacuumeOK,0;
MoveL pRPlaceSafety, v1000, fine, tVacuume;
ELSEIF i>LHNum+LVNum AND i<=2*(LHNum+LVNum)-LVNum THEN
pLPickSafety:=Offs(Offs(pLPick,-W*(i-LHNum-LVNum-1),W,-H*(Layer-2)),0,0,300);
MoveJ pLPickSafety, v1500, z50, tVacuume;
MoveL Offs(pLPick,-W*(i-LHNum-LVNum-1),W,-H*(Layer-2)), v1000, fine, tVacuume;
Set doVacuume;
WaitDI diVacuumeOK,1;
MoveL pLPickSafety, v1000, fine, tVacuume;
pRPlaceSafety:=Offs(Offs(pRPlace,-W*(i-LHNum-LVNum-1),-W,H*(Layer-2)),0,0,300);
MoveJ pRPlaceSafety, v1500, z50, tVacuume;
MoveL Offs(pRPlace,-W*(i-LHNum-LVNum-1),-W,H*(Layer-2)), v1000, fine, tVacuume;
Reset doVacuume;
WaitDI diVacuumeOK,0;
MoveL pRPlaceSafety, v1000, fine, tVacuume;
ELSEIF i>2*(LHNum+LVNum)-LVNum AND i<=2*(LHNum+LVNum) THEN
pLPickSafety:=Offs(Offs(pLPick90,-L*(i-2*LHNum-LVNum-1),-L,-H*(Layer-2)),0,0,300);
MoveJ pLPickSafety, v1500, z50, tVacuume;
MoveL Offs(pLPick90,-L*(i-2*LHNum-LVNum-1),-L,-H*(Layer-2)), v1000, fine, tVacuume;
Set doVacuume;
WaitDI diVacuumeOK,1;
MoveL pLPickSafety, v1000, fine, tVacuume;
pRPlaceSafety:=Offs(Offs(pRPlace90,-L*(i-2*LHNum-LVNum-1),L,H*(Layer-2)),0,0,300);
MoveJ pRPlaceSafety, v1500, z50, tVacuume;
MoveL Offs(pRPlace90,-L*(i-2*LHNum-LVNum-1),L,H*(Layer-2)), v1000, fine, tVacuume;
Reset doVacuume;
WaitDI diVacuumeOK,0;
MoveL pRPlaceSafety, v1000, fine, tVacuume;
ELSEIF i>2*(LHNum+LVNum) AND i<=3*(LHNum+LVNum)-LVNum THEN
pLPickSafety:=Offs(Offs(pLPick,-W*(i-2*LHNum-2*LVNum-1),0,-H*(Layer-1)),0,0,300);
MoveJ pLPickSafety, v1500, z50, tVacuume;
MoveL Offs(pLPick,-W*(i-2*LHNum-2*LVNum-1),0,-H*(Layer-1)), v1000, fine, tVacuume;
Set doVacuume;
WaitDI diVacuumeOK,1;
MoveL pLPickSafety, v1000, fine, tVacuume;
pRPlaceSafety:=Offs(Offs(pRPlace,-W*(i-2*LHNum-2*LVNum-1),0,H*(Layer-1)),0,0,300);
MoveJ pRPlaceSafety, v1500, z50, tVacuume;
MoveL Offs(pRPlace,-W*(i-2*LHNum-2*LVNum-1),0,H*(Layer-1)), v1000, fine, tVacuume;
Reset doVacuume;
WaitDI diVacuumeOK,0;
MoveL pRPlaceSafety, v1000, fine, tVacuume;
ELSE
pLPickSafety:=Offs(Offs(pLPick90,-L*(i-3*LHNum-2*LVNum-1),0,-H*(Layer-1)),0,0,300);
MoveJ pLPickSafety, v1500, z50, tVacuume;
MoveL Offs(pLPick90,-L*(i-3*LHNum-2*LVNum-1),0,-H*(Layer-1)), v1000, fine, tVacuume;
Set doVacuume;
WaitDI diVacuumeOK,1;
MoveL pLPickSafety, v1000, fine, tVacuume;
pRPlaceSafety:=Offs(Offs(pRPlace90,-L*(i-3*LHNum-2*LVNum-1),0,H*(Layer-1)),0,0,300);
MoveJ pRPlaceSafety, v1500, z50, tVacuume;
MoveL Offs(pRPlace90,-L*(i-3*LHNum-2*LVNum-1),0,H*(Layer-1)), v1000, fine, tVacuume;
Reset doVacuume;
WaitDI diVacuumeOK,0;
MoveL pRPlaceSafety, v1000, fine, tVacuume;
ENDIF
ENDFOR
MoveJ pHome, v1500, fine, tVacuume;
ENDPROC
ENDMODULE