MODULE Module1
!DECLARACION DE PUNTOS Y ORIENTACION DE LOS EJES
CONST robtarget INICIO:=[[-
166.850217856,381.026156209,619.355227846],[0.066329911,0.702778295,-
0.70798165,-0.021563722],[0,-1,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget
PREPARADO:=[[420.160642995,394.882194272,1071.506215832],[0,0.707106781,0.
707106781,0],[0,-1,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget
RECOGEGRAN_2:=[[333.769028758,383.882194271,857.006215832],[0,-
0.707106781,0.707106781,0],[-1,0,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget
RECOGEGRAN:=[[333.769028758,333.882194271,357.006215832],[0,-
0.707106781,0.707106781,0],[-1,0,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget INICIOPALGR:=[[150,900,350],[0,1,0,0],[1,-1,-
1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget
RECOGEPEQUE:=[[333.769028758,235.882194271,202.006215832],[0,-
0.707106781,0.707106781,0],[-1,0,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget
RECOGEPEQUE_2:=[[333.769028758,335.882194271,702.006215832],[0,-
0.707106781,0.707106781,0],[-1,0,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget INICIOPALPQ:=[[150,150,200],[0,1,0,0],[-
2,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
!INICIALIZACION DE LA MATRIZ PALETS GRANDE Y PEQUEÑO
VAR num XG:=0;
VAR num YG:=0;
VAR num ZG:=0;
VAR num XP:=0;
VAR num YP:=0;
VAR num ZP:=0;
!DEFINICION DE OFFSETS DE LAS PIEZAS EN LOS PALETS GRANDE Y
PEQUEÑO
CONST num XGR:=310;
CONST num YGR:=-310;
CONST num ZGR:=350;
CONST num XPQ:=210;
CONST num YPQ:=-210;
CONST num ZPQ:=200;
PROC ARRANQUE()
MoveJ INICIO,v1000,fine,tVacuum\WObj:=REPOSO;
SetDO MARCHA_CINTA,1;
SetDO ACTIVASENS,1;
ENDPROC
PROC PALETGR()
SetDO COGER,1;
MoveJ RECOGEGRAN,v1000,fine,tVacuum\WObj:=CINTA;
MoveJ RECOGEGRAN_2,v1000,fine,tVacuum\WObj:=CINTA;
MoveJDO INICIOPALGR,v1000,fine,tVacuum\WObj:=PALETGRAN,COGER,0;
SetDO SUELTA,1;
ENDPROC
PROC PALETPQ()
SETDO COGER,1;
MoveJ RECOGEPEQUE,v1000,fine,tVacuum\WObj:=CINTA;
MoveJ RECOGEPEQUE_2,v1000,fine,tVacuum\WObj:=CINTA;
MoveJDO INICIOPALPQ,v1000,fine ,tVacuum\WObj:=PALETPEQUE,COGER,0;
SetDO SUELTA,1;
ENDPROC
PROC main()
ARRANQUE;
Reset SUELTA;
IF SENSOGR=1 THEN
Reset MARCHA_CINTA;
PALETGR;
ENDIF
IF SENSOGR=0 AND SENSOPEQ=1 THEN
Reset MARCHA_CINTA;
PALETPQ;
ENDIF
ENDPROC
ENDMODULE
