MODULE MASTER

VAR num TIPO_PEGA:=0;

VAR num PLACE_COUNT:=0; !CONTADOR DE PLACES
TASK PERS tooldata Garra:=[TRUE,[[0,0,0],[1,0,0,0]],[153.7,[-95.2,323.8,310],
[1,0,0,0],0,0,12.433]];
CONST robtarget Home:=[[1938.26,-177.84,1512.49],[0,0.999799,-0.0200702,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P1:=[[1931.29,-1855.47,494.24],[0,0.999959,0.00902787,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P2:=[[1931.32,-1855.50,180.13],[0,0.999959,0.00903088,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P3:=[[1931.20,-1855.40,674.24],[0,0.999959,0.00902389,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P4:=[[1931.22,-1855.40,609.68],[0,0.999959,0.00902378,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P5:=[[1931.30,-1855.49,180.02],[0,0.999959,0.00902313,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P6:=[[1931.10,-1855.30,738.72],[0,0.999959,0.0090184,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P7:=[[1893.22,-1302.99,1512.59],[0,0.999799,-0.0200437,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P8:=[[1663.09,-158.75,909.72],[0,0.999946,0.0103724,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P9:=[[1663.19,-158.76,179.13],[0,0.999946,0.0103719,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P10:=[[1663.23,-158.76,602.14],[0,0.999946,0.0103783,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P11:=[[1663.09,-158.75,909.72],[0,0.999946,0.0103724,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P12:=[[2544.75,-476.12,831.47],[0,0.702967,0.711223,0],[-
1,0,-4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P13:=[[2544.72,-476.11,257.98],[0,0.702961,0.711228,0],[-
1,0,-4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P14:=[[2544.75,-476.13,831.47],[0,0.702967,0.711222,0],[-1,0,-
4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P15:=[[2544.75,-476.12,831.47],[0,0.702967,0.711223,0],[-1,0,-
4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P16:=[[2518.98,-142.06,714.52],[0,0.702949,0.71124,0],[-1,0,-
4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P17:=[[2518.95,-142.04,257.31],[0,0.70295,0.711239,0],[-1,0,-
4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P18:=[[2519.00,-142.06,365.83],[0,0.702948,0.711241,0],[-1,0,-
4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P19:=[[2518.98,-142.06,714.52],[0,0.702949,0.71124,0],[-1,0,-
4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P20:=[[2513.00,277.65,838.64],[0,0.696158,0.717889,0],[0,0,-
3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P21:=[[2513.10,277.66,238.41],[0,0.696155,0.717892,0],[0,0,-
3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P22:=[[2513.04,277.65,439.52],[0,0.696167,0.71788,0],[0,0,-
3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P23:=[[2512.99,277.65,838.64],[0,0.696158,0.717889,0],[0,0,-
3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P24:=[[2269.52,-183.91,1037.89],[0,0.999963,0.00856663,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P25:=[[2269.66,-183.93,303.68],[0,0.999959,0.00900273,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P26:=[[2269.57,-183.92,675.53],[0,0.999963,0.00855716,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget P27:=[[2269.53,-183.91,1037.89],[0,0.999963,0.0085684,0],[-
1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P28:=[[1392.01,-506.59,1006.74],[0,-0.705612,0.708598,0],[-
1,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P29:=[[1391.85,-506.54,296.03],[0,-0.705608,0.708603,0],[-1,0,-
2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P30:=[[1392.07,-506.62,695.51],[0,-0.705614,0.708597,0],[-1,0,-
2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P31:=[[1392.01,-506.60,1006.74],[0,-0.705612,0.708598,0],[-
1,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P32:=[[1422.02,-116.71,1100.34],[0,-0.70564,0.708571,0],[-1,0,-
2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P33:=[[1422.02,-116.71,402.53],[0,-0.705638,0.708573,0],[-1,0,-
2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P34:=[[1422.02,-116.71,665.76],[0,-0.70564,0.708571,0],[-1,0,-
2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P35:=[[1422.02,-116.71,1100.33],[0,-0.70564,0.708571,0],[-1,0,-
2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P36:=[[1395.71,366.50,1049.44],[0,-0.705625,0.708586,0],[0,0,-
1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P37:=[[1395.67,366.49,401.80],[0,-0.705625,0.708586,0],[0,0,-
1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P38:=[[1395.74,366.51,764.66],[0,-0.705623,0.708588,0],[0,0,-
1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget P39:=[[1395.71,366.50,1049.44],[0,-0.705624,0.708587,0],[0,0,-
1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PROC MAIN()
HOME_AUTO;! EXECUTA HOME
RESET_PROGRAMA;!ZERA VARIAVEIS
IF IG8=1 THEN MOSAICO1;ENDIF!VERIFICA MOSAICO ATUAL
ENDPROC
PROC MOSAICO1()
WaitDI IN20, 1; !AGUARDA SINAL DE QUALQUER PEGA
IF IN16=1 THEN PEGA_DOIS_SACOS_MOSAICO_1; ENDIF
IF IN17=1 THEN PEGA_UM_SACO_MOSAICO_1; ENDIF
E1_MOSAICO_1;
PILHA_MOSAICO_1;
S1_MOSAICO_1;
ENDPROC
PROC PEGA_DOIS_SACOS_MOSAICO_1()
ABRE_GARRAS;
MoveJ P1, v1000, z50, Garra; ! APROXIMACAO
MoveL P2, v1000, z50, Garra; ! PEGA
FECHA_GARRA_2_SACOS;
MoveL P3, v1000, z50, Garra; ! SAIDA
PulseDO\PLength:=1.0,OUT8; ! PULSO PEGA OK
ENDPROC
PROC PEGA_UM_SACO_MOSAICO_1()
ABRE_GARRAS;
MoveJ P4, v1000, z50, Garra; ! APROXIMACAO
MoveL P5, v1000, z50, Garra; ! PEGA
FECHA_GARRA_1_SACO;
MoveL P6, v1000, z50, Garra; ! SAIDA
PulseDO\PLength:=1.0,OUT8; ! PULSO PEGA OK
ENDPROC
PROC E1_MOSAICO_1()
MoveJ P7, v1000, z50, Garra;!PONTO INTERMEDIARIO DE SAIDA DO PEGA
ENDPROC
PROC PILHA_MOSAICO_1()!VERIFICA QUAL DEPOSITO REALIZAR
 IF PLACE_COUNT = 0 THEN PLACE1;ENDIF
IF PLACE_COUNT = 1 THEN PLACE2;ENDIF
IF PLACE_COUNT = 2 THEN PLACE3;ENDIF
IF PLACE_COUNT = 3 THEN PLACE4;ENDIF
IF PLACE_COUNT = 4 THEN PLACE5;ENDIF
IF PLACE_COUNT = 5 THEN PLACE6;ENDIF
IF PLACE_COUNT = 6 THEN PLACE7;ENDIF
IF PLACE_COUNT = 7 THEN PLACE8;ENDIF
ENDPROC
PROC PLACE1()!DOIS SACOS (DEPOSITO 1 DAS CAMADAS 1, 3 ,5 E 7)
IF TIPO_PEGA=1 Stop; !SE PARAR AQUI, REJEITAR PALLET E REINICIAR
CONTAGEM
MoveJ Offs(P8,IG4,IG5,IG6), v1000, z50, Garra;!APROXIMACAO
MoveL Offs(P9,IG4,IG5,IG6), v1000, z50, Garra;!ENTREGA
ABRE_GARRAS;
MoveL Offs(P10,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA1
MoveJ Offs(P11,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA2
Incr PLACE_COUNT;
ENDPROC
PROC PLACE2()!1 SACO (DEPOSITO 2 DAS CAMADAS 1, 3 ,5 E 7)
IF TIPO_PEGA=2 Stop; !SE PARAR AQUI, REJEITAR PALLET E REINICIAR
CONTAGEM
MoveJ Offs(P12,IG4,IG5,IG6), v1000, z50, Garra;!APROXIMACAO
MoveL Offs(P13,IG4,IG5,IG6), v1000, z50, Garra;!ENTREGA
ABRE_GARRAS;
MoveL Offs(P14,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA1
MoveJ Offs(P15,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA2
Incr PLACE_COUNT;
ENDPROC
PROC PLACE3()!1 SACO(DEPOSITO 3 DAS CAMADAS 1, 3 ,5 E 7)
IF TIPO_PEGA=3 Stop;! SE PARAR AQUI, REJEITAR PALLET E REINICIAR
CONTAGEM
MoveJ Offs(P16,IG4,IG5,IG6), v1000, z50, Garra;!APROXIMACAO
MoveL Offs(P17,IG4,IG5,IG6), v1000, z50, Garra;!ENTREGA
ABRE_GARRAS;
MoveL Offs(P18,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA1
MoveJ Offs(P19,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA2
Incr PLACE_COUNT;
ENDPROC
PROC PLACE4()!1 SACO(DEPOSITO 4 DAS CAMADAS 1, 3 ,5 E 7)
IF TIPO_PEGA=4 Stop; !SE PARAR AQUI, REJEITAR PALLET E REINICIAR
CONTAGEM
MoveJ Offs(P20,IG4,IG5,IG6), v1000, z50, Garra;!APROXIMACAO
MoveL Offs(P21,IG4,IG5,IG6), v1000, z50, Garra;!ENTREGA
ABRE_GARRAS;
MoveL Offs(P22,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA1
MoveJ Offs(P23,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA2
Incr PLACE_COUNT;
ENDPROC
PROC PLACE5()!2 SACOS (DEPOSITO 1 - CAMADAS 2, 4 ,6 E 8)
IF TIPO_PEGA=5 Stop; !SE PARAR AQUI, REJEITAR PALLET E REINICIAR
CONTAGEM
MoveJ Offs(P24,IG4,IG5,IG6), v1000, z50, Garra;!APROXIMACAO
MoveL Offs(P25,IG4,IG5,IG6), v1000, z50, Garra;!ENTREGA
ABRE_GARRAS;
MoveL Offs(P26,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA1
MoveJ Offs(P27,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA2
Incr PLACE_COUNT;
ENDPROC
 PROC PLACE6()!1 SACO (DEPOSITO 2 DAS CAMADAS 2, 4 ,6 E 8)
IF TIPO_PEGA=6 Stop; !SE PARAR AQUI, REJEITAR PALLET E REINICIAR
CONTAGEM
MoveJ Offs(P28,IG4,IG5,IG6), v1000, z50, Garra;!APROXIMACAO
MoveL Offs(P29,IG4,IG5,IG6), v1000, z50, Garra;!ENTREGA
ABRE_GARRAS;
MoveL Offs(P30,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA1
MoveJ Offs(P31,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA2
Incr PLACE_COUNT;
ENDPROC
PROC PLACE7()!!1 SACO (DEPOSITO 3 DAS CAMADAS 2, 4 ,6 E 8)
IF TIPO_PEGA=7 Stop;! SE PARAR AQUI, REJEITAR PALLET E REINICIAR
CONTAGEM
MoveJ Offs(P32,IG4,IG5,IG6), v1000, z50, Garra;!APROXIMACAO
MoveL Offs(P33,IG4,IG5,IG6), v1000, z50, Garra;!ENTREGA
ABRE_GARRAS;
MoveL Offs(P34,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA1
MoveJ Offs(P35,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA2
Incr PLACE_COUNT;
ENDPROC
PROC PLACE8()!1 SACO (DEPOSITO 4 DAS CAMADAS 2, 4 ,6 E 8)
IF TIPO_PEGA=8 Stop; ! VERIFICA SE TIPO DE PEGA É CORRETA
MoveJ Offs(P36,IG4,IG5,IG6), v1000, z50, Garra;!APROXIMACAO
MoveL Offs(P37,IG4,IG5,IG6), v1000, z50, Garra;!ENTREGA
ABRE_GARRAS;
MoveL Offs(P38,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA1
MoveJ Offs(P39,IG4,IG5,IG6), v1000, z50, Garra;!SAIDA2
Incr PLACE_COUNT;
ENDPROC
PROC S1_MOSAICO_1()
MoveJ P38, v1000, z50, Garra;!SAÍDA DO DEPÓSITO MOSAICO 1
PulseDO\PLength:=1.0,OUT9; ! PULSO FIM CICLO
ENDPROC
PROC HOME_AUTO()
MoveJ Home, v1000, z50, Garra;!HOME
ENDPROC
PROC RESET_PROGRAMA()
PLACE_COUNT := 0;
TIPO_PEGA := 0;
ENDPROC
PROC ABRE_GARRAS()
!SET E RESET DAS VALVULAS
Set OUT16;
Reset OUT17;
Set OUT18;
Reset OUT19;
Set OUT20;
Reset OUT21;
Set OUT22;
Reset OUT23;
!AGUARDA SENSORES DE ABERTO
WaitDI IN8, 1;
WaitDI IN10, 1;
WaitDI IN12, 1;
WaitDI IN14, 1;
ENDPROC
PROC FECHA_GARRA_2_SACOS()
!FECHA GARRAS INTERNAS
Reset OUT16;
 Set OUT17;
Reset OUT18;
Set OUT19;
!AGUARDA SENSORES DE FECHADO GARRAS INTERNAS
WaitDI IN9, 1;
WaitDI IN13, 1;
!FECHA GARRAS EXTERNAS
Reset OUT20;
Set OUT21;
Reset OUT22;
Set OUT23;
!AGUARDA SENSORES DE FECHADO GARRAS EXTERNAS
WaitDI IN11, 1;
WaitDI IN15, 1;
ENDPROC
PROC FECHA_GARRA_1_SACO()
!FECHA GARRAS INTERNAS
Reset OUT16;
Set OUT17;
!AGUARDA SENSORES DE FECHADO GARRAS INTERNAS
WaitDI IN9, 1;
!FECHA GARRAS EXTERNAS
Reset OUT18;
Set OUT19;
!AGUARDA SENSORES DE FECHADO GARRAS EXTERNAS
WaitDI IN11, 1;
ENDPROC
ENDMODULE