MODULE MainModule_m

!
*******************************************************************
!* ENGETEAM - Engenharia, Simulação, Automação e Robótica *
!* Modulo Principal *
!* Robô Extrator - Injetora 19 - NEMAK *
!* Autor: David Junior Araujo Abril/2012 *
!* Implantação: Rodrigo Luciano Machado Fevereiro/2013 *
!
*******************************************************************

!Variáveis (Tool, Load)

TASK PERS tooldata Gripper:=[TRUE,[[-600,0,200],[0,0,0,1]],[47.5,[283.7,-
0.8,10.3],[1,0,0,0],0,0,2.12]];
PERS loaddata Carcaça:=[14.6,[-37.3,138.2,-78.9],[1,0,0,0],0,1.9,1.4];

!Variáveis PathId
VAR pathrecid Espera_Injetora_ID;

!Variáveis Traps
VAR intnum Int_Cond_Seg_Injetora;

!Variáveis Bool Auxiliares

VAR bool b_Start:=FALSE;
PERS bool Presença_Buffer:=FALSE;
PERS bool Peça_OK_Buffer:=FALSE;
PERS bool Peça_OK_Serra:=TRUE;
PERS bool Peça_OK_Griper:=FALSE;
PERS bool Peça_NOK_Buffer:=FALSE;
PERS bool Peça_NOK_Serra:=FALSE;
PERS bool Peça_NOK_Griper:=FALSE;
PERS bool Peça_Injetora_OK:=FALSE;
PERS bool Peça_Injetora_NOK:=FALSE;
PERS bool Peça_Acabada:=FALSE;
PERS bool Presença_Canal:=TRUE;
PERS bool Resultado_Injetora_NOK:=FALSE;
PERS bool Griper_Carregado:=FALSE;
VAR bool Timeout_Espera_Injecao:=FALSE;
VAR bool Timeout_Espera_Condição_Extração:=FALSE;
VAR bool Timeout_Ejector_Executed:=FALSE;
VAR bool Timeout_Ejector_Forward:=FALSE;
VAR bool Timeout_Presença_Canal:=FALSE;
VAR bool Timeout_Aprovação:=FALSE;
VAR bool Timeout_Check_Sensor_Canal:=FALSE;
VAR bool Timeout_Espera_Condição_Serra:=FALSE;

!Variáveis Posição Extração

VAR robtarget Temp_Extração;

!Variáveis Numéricas Auxiliares

PERS num Rob_Pos;

!Posições para World Zones

PERS pos pos_Injetora:=[-2364,-1873,1161];
PERS pos pos_Home:=[1413.26,-199.07,1360];
PERS pos pos_Serra_WZ:=[-1462.85,1845.29,833.18];
PERS pos pos_Backup_WZ:=[1327.25,2497.41,-221.79];

!Wzstationary para World Zones

PERS wzstationary wz_esf_Home:=[1];
PERS wzstationary wz_esf_Injetora:=[2];
PERS wzstationary wz_esf_Serra:=[3];
PERS wzstationary wz_esf1_Backup:=[4];
 PERS wzstationary wz_esf2_Backup:=[5];

!ShapeData para World Zones

VAR shapedata esf_Home;
VAR shapedata esf_Injetora;
VAR shapedata esf_Serra;
VAR shapedata esf_Backup;

!Variáveis Work Object

TASK PERS wobjdata wobj_Injetora:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[-
630.479,1912.54,702.878],[0.445175,0.00217596,0.00529812,-0.895425]]];
TASK PERS wobjdata wobj_Serra:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],
[[1179.75,-1911.81,803.827],[0.69759,-0.00418934,-0.0121571,0.716381]]];
TASK PERS wobjdata wobj_Buffer:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],
[[1179.75,-1911.81,803.827],[0.69759,-0.00418934,-0.0121571,0.716381]]];
TASK PERS wobjdata wobj_Quebra_Bolsas:=[FALSE,TRUE,"",[[0,0,0],
[1,0,0,0]],[[1179.75,-1911.81,803.827],[0.69759,-0.00418934,-0.0121571,0.716381]]];
TASK PERS wobjdata wobj_Mesa:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],
[[1179.75,-1911.81,803.827],[0.69759,-0.00418934,-0.0121571,0.716381]]];
TASK PERS wobjdata wobj_Banho:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],
[[1179.75,-1911.81,803.827],[0.69759,-0.00418934,-0.0121571,0.716381]]];

!Posições Home
CONST robtarget P_Home:=[[1413.26,-199.07,1360],
[0.00404294,-0.925544,0.378618, -0.000880195],[0,0,2,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

P H := [[1413.26,-199.07, 1360], [0.088, -0.925, 0.378, 0.044] , [0,0,2,0], [9E+09,.......]

X Y Z ORIENTAÇÃO ANG. EIXOS ADC

!Posições Buffer
 CONST robtarget Pos_Acesso_Buffer:=[[471.47,1602.41,796.65],
[0.0108782,0.937727,-0.346595,0.0205546],[0,-1,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Aprox_Buffer:=[[479.28,1607.82,420.96],
[0.00782431,0.967275,-0.25361,-0.00022401],[1,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Retirada_Buffer:=[[484.02,1616.60,122.43],
[0.0193478,0.971904,-0.234245,0.0125407],[1,-1,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Depósito_Buffer:=[[486.43,1614.27,132.74],
[0.0078197,0.975499,-0.219865,-0.000490758],[1,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!Posições Mesa
CONST robtarget Pos_Acesso_Mesa:=[[315.36,2283.44,1012.77],[0.00288459,-
0.879759,-0.475411,0.000710975],[1,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Aprox_Mesa:=[[199.94,2522.16,575.35],
[0.00137276,0.700879,0.713245,0.00693788],[0,-1,-1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Mesa:=[[205.71,2526.08,82.47],
[0.00138756,0.700871,0.713253,0.00692614],[0,-1,-1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!Posições Descarte
CONST robtarget Pos_Descarte:=[[1770.57,-552.00,903.09],
[0.0494314,0.590134,-0.777511,-0.211601],[-1,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Acesso_Descarte:=[[1509.35,-115.34,1167.05],
[0.00134758,-0.929709,0.368281,0.00267013],[0,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!Posições Banho
CONST robtarget Pos_Banho_Canal:=[[1617.65,899.78,-252.22],
[0.167854,0.914928,-0.363989,-0.0473686],[0,-1,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget Pos_Retirada_Agua:=[[1609.67,1049.39,291.97],
[0.824905,0.441889,0.346441,-0.0651494],[0,-1,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Acesso_Banho:=[[1549.55,673.48,1115.91],
[0.00134512,-0.929711,0.368278,0.00267666],[0,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Banho:=[[1617.63,899.76,-252.19],
[0.0148788,0.929569,-0.368297,0.00608794],[0,-1,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Aprox_Banho:=[[1640.57,913.21,52.51],[0.0013586,-
0.929715,0.368267,0.00267219],[0,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!Posições Serra
CONST robtarget Pos_Espera_Serra:=[[-932.97,1208.59,1118.82],
[0.00177375,0.356009,0.934476,0.00307938],[1,-1,-1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Entrada_Serra:=[[-1977.4,-1298.84,1566.6],
[0.675213,0.705739,0.0495068,0.208732],[-2,1,3,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Serra:=[[-1549.74,2387.11,694.88],
[0.170518,0.371981,0.599226,0.6881],[1,-2,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Acesso_Serra:=[[-1462.85,1845.29,833.18],
[0.148136,0.390675,0.595656,0.686019],[1,-1,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Aprox_Serra:=[[-1549.74,2387.11,734.28],
[0.17052,0.371981,0.599226,0.6881],[1,-2,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Fora_Peça_Serra:=[[-1678.51,2287.68,705.51],
[0.175138,0.386955,0.606927,0.671738],[1,-1,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!Posições Backup
CONST robtarget Pos_Espera_Backup:=[[572.69,1736.66,824.68],
[0.00114351,-0.937569,-0.347782,-0.00336269],[0,-1,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget Pos_Entrada_Backup:=[[-1977.4,-1298.84,1566.6],
[0.675213,0.705739,0.0495068,0.208732],[-2,1,3,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Backup:=[[1327.25,2497.41,-221.79],[0.00688587,-
0.857515,-0.514187,-0.0152624],[0,-1,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Acesso_Backup:=[[1133.84,2221.00,421.30],
[0.00112196,-0.93757,-0.347779,-0.00334469],[0,-1,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!Posições Injetora
CONST robtarget Preparacao_Injetora:=[[441.51,637.3,1686.23],[0.0376818,-
0.983297,-0.178066,-0.000310875],[0,0,2,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Entrada_Injetora:=[[-948.92,-1039.62,1349.86],
[0.00326048,-0.339797,0.94049,-0.00253887],[-2,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Acesso_Extracao:=[[-2323.89,-1870.82,1195.09],
[0.352048,0.16862,-0.690296,-0.609197],[-2,-1,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Injetora_2:=[[-1063.64,2623.95,1683.61],
[0.648495,0.482721,-0.519586,-0.276522],[1,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Extração_Injetora:=[[-2579.81,-1603.45,1191.38],
[0.296848,0.228947,-0.672702,-0.637916],[-2,-2,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Check_Presenca_Peça:=[[90.97,-1211.21,1474.93],
[0.00195464,0.0673179,0.997723,0.00373481],[-1,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Saida_Injetora:=[[-614.36,-669.89,1553.18],
[0.00417477,-0.396088,0.918203,0.00062763],[-2,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Espera_Injetora:=[[65.79,-656.01,1350.53],
[0.00351323,-0.396118,0.91819,0.00217781],[-1,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Acesso_Check_Presenca_Peça:=[[-203.40,-664.13,1555.21],
[0.00334132,-0.358356,0.933575,0.00254445],[-1,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget Saida_Check_Presenca_Peça:=[[62.52,-923.68,1445.83],
[0.00200906,0.0540872,0.998527,0.0036893],[-1,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!Posições Intermediárias
CONST robtarget Inter_Serra_Buffer:=[[-573.05,-1313.94,1497.21],
[0.00217981,-0.769068,-0.639079,-0.0104025],[-2,0,3,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Inter_Buffer_Rebarb:=[[1025.39,-1001.92,1496.97],
[0.0077994,-0.269925,-0.962821,-0.00737597],[-1,0,3,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Inter_Injetora_Rebarb:=[[1403.08,204,1585],[0.00528913,-
0.408948,-0.912527,0.00528741],[0,-1,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!Posições Quebra_Bolsas
CONST robtarget Pos_Acesso_Quebra_Bolsas:=[[1809.53,-533.76,1359.58],
[0.00404456,-0.925541,0.378626,-0.000883651],[-1,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Aprox_Quebra_Bolsas:=[[2122.97,-1036.73,1232.86],
[0.0215423,-0.924595,0.378025,0.0419027],[-1,0,2,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Aprox_Descarte:=[[1700.62,-385.34,1021.10],
[0.0329,0.659372,-0.743775,-0.104615],[0,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS num Lubri_Garra:=0;
PERS bool Cont_Lubr:=FALSE;
PERS num Cont_Lubri:=32;
CONST robtarget Pos_Lub_p1:=[[253.32,2323.88,1127.13],[0.00290843,-
0.879762,-0.475404,0.000698668],[1,0,2,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Lub_p2:=[[246.43,2178.2,1004.75],[0.649305,-
0.333037,0.32274,0.602767],[0,1,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Lub_p3:=[[793.76,2882.69,181.11],[0.69263,-
0.317167,0.377235,0.526653],[0,1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Lub_p4:=[[246.46,2178.16,1004.74],[0.649305,-
0.333041,0.322745,0.602762],[0,1,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget Pos_Lub_p5:=[[253.39,2323.9,1127.05],[0.00289075,-
0.879775,-0.475381,0.000709036],[1,0,2,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Lub_p6:=[[520.39,1751.72,1123.54],[0.00211179,-
0.998273,0.0586704,0.00212517],[1,0,3,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Lub_p7:=[[-44,2421.14,1128.71],[0.00297734,-
0.725391,-0.688331,-7.53164E-05],[1,0,2,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Chill_p1:=[[2395.18,-1205.32,1173.73],[0.00381052,-
0.97943,0.201741,-0.00163117],[-1,0,-1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Chill_p2:=[[2220.55,-895.82,1202.34],[0.704169,-
0.51426,0.489059,0.0224195],[-1,1,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Chill_p3:=[[2404.73,-1061.27,1110.85],[0.704171,-
0.514271,0.489046,0.0224084],[-1,1,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Chill_p4:=[[2421.36,-1081.07,1096.93],[0.697496,-
0.552194,0.456086,-0.023814],[-1,1,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Chill_p5:=[[2617.40,-850.61,1066.83],[0.696898,-
0.560762,0.445551,-0.0369192],[-1,1,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Chill_p6:=[[2421.35,-1081.14,1087.79],[0.697492,-
0.552186,0.456101,-0.0238134],[-1,1,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Chill_p7:=[[2061.45,-1041.35,1369.26],[0.00408234,-
0.904586,0.426271,-0.000655594],[-1,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Extração_Injetora10:=[[-614.36,-669.89,1553.18],
[0.00417587,-0.396089,0.918202,0.000628157],[-2,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Fora_Peça_Serra10:=[[-1574.87,2380.15,687.45],
[0.196159,0.371516,0.604932,0.676427],[1,-2,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Espera_Injetora10:=[[-184.09,-1009.42,1360.29],
[0.00402911,-0.37262,0.927975,0.000194171],[-1,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS num Lubri_Garr:=0;
 CONST robtarget Acesso_Check_Presenca_Peça10:=[[62.52,-923.68,1445.83],
[0.00200956,0.0540875,0.998527,0.00368858],[-1,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Saida_Check_Presenca_Peça10:=[[464.94,-700.45,1481.50],
[0.00232545,-0.304124,0.952627,0.00221835],[-1,0,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Aprox_Mesa10:=[[755.42,2866.23,-95.83],[0.0084814,-
0.919245,-0.393261,0.0161792],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Acesso_Buffer10:=[[615.65,1977.72,788.11],
[0.0171372,0.999654,-0.0123058,0.01572],[0,-1,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Acesso_Buffer20:=[[471.48,1602.41,796.65],
[0.010877,0.937726,-0.346596,0.0205542],[0,-1,1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Espera_Serra10:=[[-932.96,1208.60,1118.81],
[0.00177683,0.356013,0.934474,0.00307692],[1,-1,-1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Pos_Espera_Serra20:=[[-932.96,1208.59,1118.87],
[0.00175904,0.356017,0.934473,0.00309083],[1,-1,-1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

PROC Define_Zonas()
!Define world zones para Home
!Define pos Home a partir do Robtarget
pos_Home:=[P_Home.trans.x,P_Home.trans.y,P_Home.trans.z];
!Define tamanho e tipo do WorkZone
WZSphDef\Inside,esf_Home,pos_Home,20;
!Define o acionamento de uma DO
WZDOSet\Stat, wz_esf_Home\Inside, esf_Home, doDCMInBasePos, 1;

!Define world zones para Injetora

!Define tamanho e tipo do WorkZone
WZSphDef\Inside, esf_Injetora, pos_Injetora, 2000;
!Define o acionamento de uma DO
 WZDOSet\Stat, wz_esf_Injetora\Inside, esf_Injetora, doDCMInDanger, 1;

!Define world zones para Serra

!Define pos Home a partir do Robtarget

pos_Serra_WZ:=[Pos_Acesso_Serra.trans.x,Pos_Acesso_Serra.trans.y,Pos_Acesso_Ser
ra.trans.z];
!Define tamanho e tipo do WorkZone
WZSphDef\Inside,esf_Serra,pos_Serra_WZ,1000;
!Define o acionamento de uma DO
WZDOSet\Stat, wz_esf_Serra\Inside, esf_Serra, doOutserra, 0;

!Define world zones para Mesa Backup

!Define pos Home a partir do Robtarget
pos_Backup_WZ:=[Pos_Backup.trans.x,Pos_Backup.trans.y,Pos_Backup.trans.z];
!Define tamanho e tipo do WorkZone
WZSphDef\Inside, esf_Backup, pos_Backup_WZ, 1000;
!Define o acionamento de uma DO
WZDOSet\Stat, wz_esf1_Backup\Inside, esf_Backup, do1OutBarreira, 0;
WZDOSet\Stat, wz_esf2_Backup\Inside, esf_Backup, do2OutBarreira, 0;

Stop;
ENDPROC

PROC Move_to_Home()
MoveJ P_Home, v7000, fine, Gripper;
!
ENDPROC

PROC CheckHomePos()
! Verifica se o robô está na posição HOME.
 !Caso não esteja, movimenta para a posição HOME se estiver em uma região
conhecida.
WHILE DOutput(doDCMInBasePos) = 0 DO
IF DOutput(doDCMInBasePos) = 0 THEN
!Gestão_Falhas 15;
TPErase;
TPWrite "Robo está em uma zona desconhecida";
WaitTime 2;
TPReadFK reg1, "Robô está fora de uma area com possível
trajetória de colisão?", "SIM", "", "", "", "NAO";
TEST reg1
CASE 1:
IF DOutput(Manual_Mode) = 1 THEN
Move_to_Home;
ELSE
TPWrite "Robô não está em Manual";
WaitTime 2;
Stop;
ENDIF
CASE 5:
TPErase;
TPWrite "Colocar o robô próximo da HOME";
WaitTime 2;
ENDTEST
ENDIF
ENDWHILE
IF DOutput(doDCMInBasePos) = 1 THEN
b_Start:=TRUE;
ENDIF
WaitTime 1;
ENDPROC
 PROC main()
! PECA CARCACA ONIX
WaitTime 0.5;
IF b_Start=FALSE THEN
Abrir_Griper;
ENDIF
!Rotina para controle Inicial
IF b_Start=FALSE THEN
CheckHomePos;
!Carrega rampas para manipulação sem Carcaça
VelSet 100,4000;
Accset 85, 85;
TPErase;
ENDIF
!Condições para aproximação e extração da injetora
IF diDCM_SA_A=1 AND diDCMExtract1=0 AND diDCMExtract2=0 AND
diPresença_Buffer=0 AND Griper_Carregado=FALSE AND
DOutput(doDCMInBasePos) = 1 THEN
MoveL Pos_Espera_Injetora, v2000, fine, Gripper;
Abrir_Griper;
Extração_Injetora;
ENDIF
!Condições para extração da injetora
IF diDCM_SA_A = 1 AND diDCMRelCycl = 1 AND diDCMExtract1 = 1 AND
diDCMExtract2 = 1 AND diPresença_Buffer = 0 AND Griper_Carregado = FALSE
AND DOutput(doDCMInBasePos) = 1 THEN
Abrir_Griper;
Extração_Injetora;
ENDIF
!Condições para descarregamento de peça do buffer
 IF diPresença_Buffer = 1 AND (diSerraAuto = 0 OR (diSerraAuto = 1 AND
diSerra_Carga = 1)) AND Griper_Carregado = FALSE AND
DOutput(doDCMInBasePos) = 1 THEN
Buffer_Retirada;
IF Peça_OK_Griper = TRUE THEN
Mesa_Depósito;
Move_to_Home;
ExitCycle;
ELSEIF Peça_NOK_Griper = TRUE THEN
Descarte;
Move_to_Home;
ExitCycle;
ENDIF
ENDIF
!GOTO Fim;
!Condições para ciclo da mesa de backup
IF diDCM_SA_A = 0 AND diBarreiraAtuada = 0 AND diPresença_Backup = 1
AND DOutput(doDCMInBasePos) = 1 AND diSerraAuto = 1 AND (diSerra_Carga = 1
OR diSerra_Desc = 1) THEN
Abrir_Griper;
MoveJ Pos_Acesso_Buffer, v4000, z50, Gripper;
Inicio_Backup:
IF diSerraAuto = 1 AND diSerra_Desc = 1 THEN
Serra_Retirada;
Mesa_Depósito;
ELSEIF diBarreiraAtuada = 0 AND diPresença_Backup = 1 AND
diSerraAuto = 1 AND diSerra_Carga = 1 THEN
Backup_Retirada;
IF diSerraAuto = 1 AND diSerra_Carga = 1 THEN
Serra_Depósito;
ELSE
Mesa_Depósito;
 ENDIF
ELSE
Move_to_Home;
ExitCycle;
ENDIF
!
WaitUntil diDCM_SA_A = 1 OR diSerraAuto = 0 OR ((diSerra_Carga =
1 OR diSerra_Desc = 1) AND diPresença_Backup = 0) OR (diDCM_SA_A = 0 AND
diBarreiraAtuada = 0 AND diPresença_Backup = 1 AND (diSerra_Carga = 1 OR
diSerra_Desc = 1))\MaxTime:=90\TimeFlag:=Timeout_Espera_Condição_Serra;
!
IF (diSerraAuto = 1 AND (diSerra_Desc = 1 OR (diSerra_Carga = 1
AND diDCM_SA_A = 0 AND diBarreiraAtuada = 0 AND diPresença_Backup = 1)))
THEN
GOTO Inicio_Backup;
ELSE
Move_to_Home;
ExitCycle;
ENDIF
ENDIF
Fim:
ENDPROC

PROC Buffer_Depósito ()
MoveL Pos_Acesso_Buffer, v7000, z200, Gripper;
MoveL Pos_Aprox_Buffer, v3000, z1, Gripper;
WaitDI diPresença_Buffer, 0;
MoveL Pos_Depósito_Buffer, v150, fine, Gripper;
Abrir_Griper;
Check_Griper_Aberto;
IF Peça_NOK_Griper = TRUE THEN
Peça_NOK_Buffer:=TRUE;
 Peça_OK_Buffer:=FALSE;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=FALSE;
ELSEIF Peça_OK_Griper = TRUE THEN
Peça_NOK_Buffer:=FALSE;
Peça_OK_Buffer:=TRUE;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=FALSE;
ENDIF
MoveL Pos_Aprox_Buffer, v7000, z50, Gripper;
MoveL Pos_Acesso_Buffer, v7000, z50, Gripper;
MoveL [[615.65,1977.71,788.11],[0.0171381,0.999654,-
0.0123069,0.0157207],[0,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v4000,
z50, Gripper;
ENDPROC

PROC Buffer_Retirada ()
Abrir_Griper;
MoveL Pos_Acesso_Buffer, v7000, z200, Gripper;
MoveL Pos_Aprox_Buffer, v7000, fine, Gripper;
WaitDI diPresença_Buffer, 1;
MoveL Pos_Retirada_Buffer, v200, fine, Gripper;
Fechar_Griper;
Check_Griper_Fechado;
IF Peça_NOK_Buffer = TRUE THEN
Peça_NOK_Buffer:=FALSE;
Peça_OK_Buffer:=FALSE;
Peça_NOK_Griper:=TRUE;
Peça_OK_Griper:=FALSE;
ELSEIF Peça_OK_Buffer = TRUE THEN
Peça_NOK_Buffer:=FALSE;
 Peça_OK_Buffer:=FALSE;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=TRUE;
ENDIF
MoveL Pos_Aprox_Buffer, v400, fine, Gripper;
WaitDI diPresença_Buffer, 0;
MoveL Pos_Acesso_Buffer, v1000, z50, Gripper;
MoveL [[592.80,1828.18,792.58],[0.0150196,0.990121,-
0.138273,0.0177566],[0,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000,
z50, Gripper;
ENDPROC

PROC Buffer_Referência ()
MoveL Pos_Acesso_Buffer, v4000, z50, Gripper;
MoveL Pos_Aprox_Buffer, v4000, z50, Gripper;
WaitDI diPresença_Buffer, 0;
MoveL Pos_Depósito_Buffer, v100, fine, Gripper;
Abrir_Griper;
Check_Griper_Aberto;
IF Peça_NOK_Griper = TRUE THEN
Peça_NOK_Buffer:=TRUE;
Peça_OK_Buffer:=FALSE;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=FALSE;
ELSEIF Peça_OK_Griper = TRUE THEN
Peça_NOK_Buffer:=FALSE;
Peça_OK_Buffer:=TRUE;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=FALSE;
ENDIF
MoveL Pos_Retirada_Buffer, v80, fine, Gripper;
 WaitTime 1;
Fechar_Griper;
Check_Griper_Fechado;
IF Peça_NOK_Buffer = TRUE THEN
Peça_NOK_Buffer:=FALSE;
Peça_OK_Buffer:=FALSE;
Peça_NOK_Griper:=TRUE;
Peça_OK_Griper:=FALSE;
ELSEIF Peça_OK_Buffer = TRUE THEN
Peça_NOK_Buffer:=FALSE;
Peça_OK_Buffer:=FALSE;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=TRUE;
ENDIF
MoveL Pos_Aprox_Buffer, v80, z50, Gripper;
WaitDI diPresença_Buffer, 0;
MoveL Pos_Acesso_Buffer, v4000, z50, Gripper;
ENDPROC

PROC Quebra_Bolsas()
MoveL Pos_Acesso_Quebra_Bolsas, v4000, z50, Gripper;
!Bolsas Inferiores
MoveL [[2243.34,-903.87,1505.11],[0.00408662,-0.908627,0.417588,-
0.000622762],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v4000, z100,
Gripper;
MoveL [[2317.07,-1136.94,1373.24],[0.00401701,-0.938517,0.345208,-
0.00102316],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
MoveL [[2317.42,-1136.43,1297.81],[0.00401756,-0.938517,0.345208,-
0.00102021],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
 MoveL [[2340.30,-1132.78,1373.49],[0.00381283,-0.97943,0.201743,-
0.00162889],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
MoveL [[2529.05,-1077.35,1300.81],[0.00403537,-0.927907,0.372788,-
0.00101494],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
MoveL [[2529.46,-1076.78,1221.15],[0.00403419,-0.927911,0.372779,-
0.000996899],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v400, z1,
Gripper;
MoveL [[2484.12,-1092.23,1341.33],[0.00380278,-0.979428,0.201752,-
0.00166644],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
!Bolsas Superiores
MoveL [[2449.57,-1009.16,1124.91],[0.040675,-
0.811679,0.578943,0.0659312],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v2000, z1, Gripper;
MoveL [[2432.35,-999.39,928.76],[0.0406721,-
0.811678,0.578945,0.0659374],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v400, z1, Gripper;
MoveL [[2381.26,-1065.35,1302.88],[0.0452959,-
0.851164,0.51915,0.0628676],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v2000, z1, Gripper;
MoveL [[2399.79,-1018.53,910.59],[0.0407232,-
0.812216,0.578188,0.0659158],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v1000, fine, Gripper;
MoveL [[2365.81,-980.55,1311.15],[0.0452799,-
0.851164,0.519149,0.0628766],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v2000, fine, Gripper;
MoveL [[2392.40,-1031.92,1302.28],[0.853329,-0.272809,0.410105,-
0.170932],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v2000, z1, Gripper;
MoveL [[2392.42,-1031.98,1302.19],[0.0453054,-
0.851161,0.519157,0.0628404],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v2000, z1, Gripper;
MoveL [[2320.38,-1046.17,1378.97],[0.0204883,0.693094,-0.714548,-
0.0928536],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
MoveL [[2337.56,-1043.92,1023.62],[0.020505,0.692976,-0.714662,-
0.0928534],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v500, fine,
Gripper;
 MoveL [[2331.64,-1045.97,1234.18],[0.0205016,0.692978,-0.714662,-
0.0928461],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v600, z1, Gripper;
MoveL [[2320.38,-1046.17,1378.97],[0.0906187,-0.8331,0.543682,-
0.0462897],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
MoveL [[2352.46,-1033.93,1033.76],[0.0906186,-0.8331,0.543683,-
0.0462843],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v300, fine,
Gripper;
MoveL [[2320.38,-1046.17,1378.98],[0.0906192,-0.833101,0.543681,-
0.0462916],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
MoveL [[2333.21,-1014.27,1026.93],[0.0905849,-0.833104,0.54369,-
0.0461866],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v300, z1, Gripper;
MoveL [[2354.79,-1007.93,1177.54],[0.0434773,-0.85241,0.520715,-
0.0190623],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v2000, z1,
Gripper;
MoveL [[2358.17,-1002.50,1452.78],[0.804344,-0.318788,0.309623,-
0.394384],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v2000, z1, Gripper;
!MoveL [[2178.68,-1157.37,1301.34],[0.710238,-0.385948,0.295417,-
0.50925],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2062.71,-786.15,1330.67],[0.710246,-0.38593,0.295398,-
0.509263],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2189.65,-974.67,1434.58],[0.710235,-0.385943,0.295408,-
0.509261],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
MoveL [[2239.48,-1070.31,1390.06],[0.644771,-0.634782,0.368795,-
0.212869],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
MoveL [[2251.89,-1064.83,1178.10],[0.644771,-0.634781,0.368788,-
0.212883],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v400, z1, Gripper;
MoveL [[2331.43,-1016.22,1333.47],[0.553075,-0.636613,0.464553,-
0.270226],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
MoveL [[2358.16,-1002.50,1327.03],[0.706263,-0.487458,0.310702,-
0.408708],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2358.16,-1002.50,1327.03],[0.706263,-0.487458,0.310702,-
0.408707],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
MoveJ [[2331.42,-1016.26,1333.53],[0.553075,-0.636609,0.464549,-
0.270242],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
MoveL [[2098.00,-1026.45,1444.77],[0.612413,-0.671293,0.189289,-
0.372137],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
 MoveL [[2097.33,-1027.07,1300.85],[0.612413,-0.671294,0.189291,-
0.372134],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
MoveL [[2214.01,-918.39,1305.16],[0.612411,-0.671296,0.189292,-
0.372133],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2294.51,-1011.68,1117.84],[0.0368636,-0.754325,0.653317,-
0.0530152],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
!MoveL [[2292.60,-1010.17,1009.35],[0.0368626,-0.754324,0.653319,-
0.0530131],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
MoveL [[2367.51,-1097.54,1442.36],[0.0288263,-
0.967901,0.249136,0.0163796],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v2000, z1, Gripper;
!MoveJ [[2041.53,-1147.03,1277.76],[0.547574,-0.563847,0.355032,-
0.506154],[-1,1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2236.32,-910.15,1017.24],[0.022034,-0.524027,0.844303,-
0.10983],[-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2236.32,-910.15,1072.45],[0.0220358,-0.524027,0.844303,-
0.109832],[-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2357.39,-1058.21,966.61],[0.0288265,-
0.967901,0.249136,0.0163819],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v1000, fine, Gripper;
!MoveL [[2215.77,-895.50,1072.46],[0.0255113,-0.496987,0.860497,-
0.10908],[-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2362.22,-1076.03,1244.51],[0.0288268,-
0.9679,0.249138,0.0163816],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v2000, fine, Gripper;
!MoveL [[2257.91,-898.88,1073.79],[0.0251355,-0.545265,0.829337,-
0.11939],[-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2261.83,-927.22,1245.70],[0.0192007,-0.545507,0.830589,-
0.110344],[-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2281.37,-952.22,1541.32],[0.458977,-0.288218,0.394012,-
0.742311],[-1,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v2000, z1, Gripper;
!MoveL [[2162.28,-901.85,1244.00],[0.253271,-0.358246,0.665658,-
0.603667],[-1,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, fine,
Gripper;
!MoveL [[2115.07,-858.46,1283.02],[0.281482,-0.329172,0.638701,-
0.635984],[-1,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
 !MoveL [[2143.95,-888.09,1310.78],[0.174337,-0.389306,0.595314,-
0.680918],[-1,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2117.39,-810.98,1413.38],[0.292262,-0.318208,0.644235,-
0.631101],[-1,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2143.95,-888.09,1442.64],[0.174343,-0.3893,0.59531,-
0.680923],[-1,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2288.61,-965.07,1398.01],[0.416651,-0.344882,0.472511,-
0.695839],[-1,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveJ [[2140.80,-940.54,1729.88],[0.457015,-0.135648,0.326435,-
0.816197],[-1,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
WaitTime 0;
!MoveJ [[2112.88,-937.11,1514.38],[0.415075,-0.228754,0.392838,-
0.788075],[-1,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2259.86,-910.94,1242.79],[0.0192253,-0.545501,0.830589,-
0.110369],[-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
!MoveL [[2262.47,-924.04,1202.77],[0.0192253,-0.545489,0.830596,-
0.110372],[-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v2000, z1, Gripper;
MoveL [[2194.56,-851.62,1500.47],[0.0192624,-0.545486,0.830597,-
0.110375],[-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
MoveL [[2302.06,-1024.04,1443.91],[0.00681247,-
0.895501,0.444977,0.00515414],[-1,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v2000, z1, Gripper;
MoveJ [[2302.06,-1024.03,1443.93],[0.00681453,-
0.895502,0.444975,0.00514849],[-1,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
MoveJ [[2233.51,-892.51,2017.63],[0.913787,0.111142,0.236095,-
0.311289],[-1,-2,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v2000, z1,
Gripper;
MoveL [[2233.45,-892.56,2017.60],[0.952071,0.147841,0.215043,-
0.159565],[-1,-2,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1, Gripper;
MoveL [[2233.11,-892.26,2018.49],[0.916388,0.113087,0.234981,-
0.303692],[-1,-2,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, z1,
Gripper;
MoveJ [[2302.07,-1024.03,1443.93],[0.00681474,-
0.895503,0.444974,0.00514744],[-1,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v2500, z1, Gripper;
MoveL Pos_Acesso_Quebra_Bolsas, v4000, z100, Gripper;
 !
ENDPROC

PROC Mesa_Depósito()
Rob_Pos:=10;
MoveL Pos_Acesso_Mesa, v7000, z200, Gripper;
MoveL Pos_Aprox_Mesa, v4000, z100, Gripper;
WaitUntil diPresença_mesa = 0;
MoveL Pos_Mesa, v500, fine, Gripper;
WaitTime 1;
Abrir_Griper;
Check_Griper_Aberto;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=FALSE;
MoveL offs (Pos_Mesa,0,0,200), v500, z50, Gripper;
MoveL Pos_Acesso_Mesa, v4000, z200, Gripper;
Rob_Pos:=10;
Lubrifica_Garra;
ENDPROC

PROC Descarte()
Rob_Pos:=4;
MoveJ Pos_Acesso_Descarte, v2500, z50, Gripper;
MoveL Pos_Aprox_Descarte, v2500, z50, Gripper;
MoveL Pos_Descarte, v600, fine, Gripper;
Abrir_Griper;
Check_Griper_Aberto;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=FALSE;
MoveL Offs(Pos_Descarte,0,0,200), v500, z50, Gripper;
MoveL Pos_Acesso_Descarte, v2500, z50, Gripper;
 Rob_Pos:=4;
ENDPROC

PROC Backup_Retirada()
!Rob_Pos:=4;
MoveJ Pos_Espera_Backup, v2500, z50, Gripper;
WaitUntil diPresença_Backup = 1 AND diBarreiraAtuada = 0;
MoveJ Pos_Acesso_Backup, v2500, z50, Gripper;
MoveL Offs(Pos_Backup,0,0,200), v500, z50, Gripper;
MoveL Pos_Backup, v500, fine, Gripper;
Fechar_Griper;
MoveL Offs(Pos_Backup,0,0,350), v80, z50, Gripper;
MoveL Pos_Acesso_Backup, v2500, z50, Gripper;
MoveJ Pos_Espera_Backup, v2500, z50, Gripper;
!Rob_Pos:=4;
ENDPROC

PROC Injetora()
WaitUntil (diDCM_SA_A = 1 AND diDCMRelCycl = 1 AND diDCMExtract1
= 1 AND diDCMExtract2 = 1) OR diDCM_SA_A = 0;
IF diDCM_SA_A = 0 THEN
Move_to_Home;
ExitCycle;
ENDIF
WaitUntil diDCM_SA_A = 1 AND diDCMRelCycl = 1 AND diDCMExtract1 =
1 AND diDCMExtract2 = 1;
Carrega_Trap_Seg_Inj;
MoveL Pos_Espera_Injetora, v4000, fine, Gripper;
MoveJ Pos_Espera_Injetora10, v4000, fine, Gripper;
WaitUntil diDCM_SA_A = 1 AND diDCMRelCycl = 1 AND diDCMExtract1 =
1 AND diDCMExtract2 = 1;
 Carrega_Trap_Seg_Inj;
MoveJ Pos_Entrada_Injetora, v4000, fine, Gripper;
MoveL Pos_Acesso_Extracao, v4000, z50, Gripper;
Check_Ejetor:
IF diDCMEjectBwd = 0 THEN
Gestão_Falhas 1;
GOTO Check_Ejetor;
ENDIF
Check_Griper_Aberto;
WaitUntil diDCMApproach =
1\MaxTime:=8\TimeFlag:=Timeout_Espera_Condição_Extração;
Descarrega_Trap_Seg_Inj;
IF Timeout_Espera_Condição_Extração = FALSE THEN
MoveL Pos_Extração_Injetora, v300, fine, Gripper;
Fechar_Griper;
Check_Griper_Fechado;
CSSAct\RefFrame:=CSS_REFFRAME_TOOL,
CSS_Z\StiffnessNonSoftDir:=50\Stiffness:=50\AllowMove;
!SoftAct 1, 100;
!SoftAct 4, 85;
!SoftAct 5, 85;
!SoftAct 6, 50;
Set doDCMEjectFwd;
Check_Avanço_Ejetor:
WaitDI diDCMEjectFwd,
1\MaxTime:=20\TimeFlag:=Timeout_Ejector_Forward;
IF diDCMEjectBwd = 1 THEN
!SoftDeact;
Temp_Extração:=CRobT(\Tool:=Gripper);
CSSDeactMoveL Temp_Extração, v100, Gripper;
Reset doDCMEjectFwd;
Set doDCMEjectBwd;
 Abrir_Griper;
Check_Griper_aberto;
MoveL Pos_Acesso_Extracao, v300, z50, Gripper;
MoveL Pos_Entrada_Injetora, v2000, z50, Gripper;
MoveJ Pos_Espera_Injetora, v2000, z50, Gripper;
MoveJ P_Home, v3000, fine, Gripper;
RETURN;
ENDIF
IF Timeout_Ejector_Forward = TRUE AND diDCMEjectFwd = 0 THEN
Gestão_Falhas 2;
GOTO Check_Avanço_Ejetor;
ELSE
WaitTime 0.2;
Temp_Extração:=CRobT(\Tool:=Gripper);
!MoveL Temp_Extração, v1000, z10, Gripper;
CSSDeactMoveL Temp_Extração, v100, Gripper;
!SoftDeact;
Reset doDCMEjectFwd;
PulseDO\PLength:=1, doDCMEjectBwd;
!Set doDCMEjectBwd;
Check_Retorno_Ejetor:
!WaitDI diDCMEjectBwd,
1\MaxTime:=5\TimeFlag:=Timeout_Ejector_Executed;
!IF Timeout_Ejector_Executed = TRUE THEN
!Gestão_Falhas 3;
!GOTO Check_Retorno_Ejetor;
!ENDIF
!Reset doDCMEjectBwd;
ENDIF
ELSE
Gestão_Falhas 4;
 WaitDI diDCMToBasePos, 1;
ENDIF
MoveL Pos_Acesso_Extracao, v2500, fine, Gripper;
MotionSup\On\TuneValue:=100;
MoveL Pos_Saida_Injetora, v4000, z50, Gripper;
IF diDCMToBasePos = 1 THEN
GOTO End_Injetora;
ENDIF
Check_Sensor_Presença:
WaitDI diPresença_Canal,
0\MaxTime:=5\TimeFlag:=Timeout_Check_Sensor_Canal;
IF Timeout_Check_Sensor_Canal = TRUE THEN
Gestão_Falhas 5;
GOTO Check_Sensor_Presença;
ENDIF
MoveJ Acesso_Check_Presenca_Peça, v4000, fine, Gripper;
MoveL Acesso_Check_Presenca_Peça10, v4000, fine, Gripper;
MoveL Pos_Check_Presenca_Peça, v3000, fine, Gripper;
WaitTime 0.5;
WaitDI diPresença_Canal,
1\MaxTime:=10\TimeFlag:=Timeout_Presença_Canal;
IF Timeout_Presença_Canal = FALSE THEN
Presença_Canal:=TRUE;
ELSE
Presença_Canal:=FALSE;
ENDIF
End_Injetora:
IF Presença_Canal = TRUE THEN
PulseDO\PLength:=5, doDCMExtracted;
ELSE
ErrWrite "Falha Presença/Aprovação de Peça durante Verificação - Peça
não completa", "Aguardando Solicitação de Home";
 SetDO doDCMFault1, 1;
Stop;
Reset doDCMFault1;
ENDIF
MoveL Saida_Check_Presenca_Peça, v4000, fine, Gripper;
MoveL Saida_Check_Presenca_Peça10, v4000, fine, Gripper;
WaitUntil diDCMSepGood=1 OR diDCMSepSample=1 OR
diDCMSepOffTol=1 OR
diDCMSepRej=1\MaxTime:=5\TimeFlag:=Timeout_Aprovação;
IF Timeout_Aprovação=FALSE THEN
IF diDCMSepGood=1 THEN
Peça_Injetora_OK:=TRUE;
Peça_Injetora_NOK:=FALSE;
ELSEIF diDCMSepSample=1 OR diDCMSepOffTol=1 OR
diDCMSepRej=1 THEN
Peça_Injetora_OK:=FALSE;
Peça_Injetora_NOK:=TRUE;
ENDIF
ELSE
Resultado_Injetora_NOK:=TRUE;
ENDIF
WaitUntil Peça_Injetora_NOK = TRUE OR Resultado_Injetora_NOK = TRUE
OR Peça_Injetora_OK = TRUE;
IF Peça_Injetora_NOK = TRUE OR Resultado_Injetora_NOK = TRUE THEN
Peça_NOK_Griper:=TRUE;
Peça_OK_Griper:=FALSE;
ELSEIF Peça_Injetora_OK = TRUE THEN
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=TRUE;
ENDIF
IF diDCMToBasePos = 1 THEN
Move_to_Home;
 RETURN;
ENDIF
!
ENDPROC

PROC Banho()
Rob_Pos:=6;
MoveL Pos_Acesso_Banho, v3000, z200, Gripper;
MoveL Pos_Aprox_Banho, v2000, z150, Gripper;
MoveL Pos_Banho, v1500, z1, Gripper;
WaitTime 2;
MoveL Pos_Banho_Canal, v2500, z10, Gripper;
WaitTime 4;
MoveL Pos_Retirada_Agua, v600, z100, Gripper;
WaitTime 6;
MoveL Pos_Acesso_Banho, v1000, z200, Gripper;
WaitTime 2;
Rob_Pos:=6;
ENDPROC

PROC Serra_Depósito ()
MoveJ Pos_Espera_Serra, v4000, z200, Gripper;
MoveL Pos_Acesso_Serra, v2500, z100, Gripper;
MoveL Pos_Aprox_Serra, v600, fine, Gripper;
MoveL Pos_Serra, v100, fine, Gripper;
Abrir_Griper;
WaitTime 1;
Check_Griper_Aberto;
IF Peça_NOK_Griper = TRUE THEN
Peça_NOK_Serra:=TRUE;
Peça_OK_Serra:=FALSE;
 Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=FALSE;
ELSEIF Peça_OK_Griper = TRUE THEN
Peça_NOK_Serra:=FALSE;
Peça_OK_Serra:=TRUE;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=FALSE;
ENDIF
MoveL Pos_Fora_Peça_Serra, v400, fine, Gripper;
MoveL Pos_Acesso_Serra, v4000, z100, Gripper;
MoveL Pos_Espera_Serra, v7000, fine, Gripper;
PulseDO\PLength:=10, doPecaINSSerra;
!
ENDPROC

PROC Serra_Retirada ()
MoveJ Pos_Espera_Serra20, v7000, z150, Gripper;
MoveL Pos_Espera_Serra, v7000, z150, Gripper;
MoveL Pos_Acesso_Serra, v7000, z150, Gripper;
MoveL Pos_Fora_Peça_Serra, v7000, z20, Gripper;
MoveL [[-1549.73,2387.10,694.82],
[0.170529,0.371972,0.599237,0.688093],[1,-2,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, fine, Gripper;
!MoveL Pos_Serra, v100, fine, Gripper;
Fechar_Griper;
WaitTime 0;
Check_Griper_Fechado;
IF Peça_NOK_Serra = TRUE THEN
Peça_NOK_Serra:=FALSE;
Peça_OK_Serra:=FALSE;
Peça_NOK_Griper:=TRUE;
 Peça_OK_Griper:=FALSE;
ELSEIF Peça_OK_Serra = TRUE THEN
Peça_NOK_Serra:=FALSE;
Peça_OK_Serra:=FALSE;
Peça_NOK_Griper:=FALSE;
Peça_OK_Griper:=TRUE;
ENDIF
MoveL Pos_Aprox_Serra, v600, z10, Gripper;
MoveL Pos_Acesso_Serra, v2500, z100, Gripper;
MoveL Pos_Espera_Serra, v7000, z1, Gripper;
PulseDO\PLength:=10, doPecaDESSerra;
!
ENDPROC

PROC Fechar_Griper()
Reset doGripper;
Griper_Carregado:=TRUE;
Gripload Carcaça;
VelSet 100, 4000;
Accset 85, 85;
WaitTime 1;
ENDPROC

PROC Abrir_Griper()
Set doGripper;
Griper_Carregado:=FALSE;
Peça_Injetora_OK:=FALSE;
Peça_Injetora_NOK:=FALSE;
Resultado_Injetora_NOK := FALSE;
GripLoad load0;
VelSet 100, 7000;
 Accset 85, 85;
WaitTime 1;
ENDPROC

PROC Check_Griper_Aberto()
Check_Griper_Aberto:
IF DOutput(doGripper)=0 THEN
ErrWrite "Griper Fechado", "Abrir Gripper de Manipulação";
SetDO doDCMFault1,1;
Stop;
Reset doDCMFault1;
GOTO Check_Griper_Aberto;
ENDIF
!
ENDPROC

PROC Check_Griper_Fechado()
Check_Griper_Fechado:
IF DOutput(doGripper)=1 THEN
ErrWrite "Falha Fechamento Gripper", "Gripper está aberto. Fechar
Gripper";
SetDO doDCMFault1,1;
Stop;
Reset doDCMFault1;
GOTO Check_Griper_Fechado;
ENDIF
ENDPROC

PROC Gestão_Falhas (num Código)

TEST Código
CASE 1:
 ErrWrite "Extrator não está em repouso", "Verificar
Extrator Injetora";
SetDO doDCMFault1,1;
Stop;
Reset doDCMFault1;
CASE 2:
ErrWrite "Falha avanço extrator", "Verifique extrator
Injetora";
SetDO doDCMFault1,1;
Stop;
Reset doDCMFault1;
CASE 3:
ErrWrite "Falha retorno extrator", "Verifique extrator
Injetora";
SetDO doDCMFault1,1;
Stop;
Reset doDCMFault1;
CASE 4:
ErrWrite "Falha Descarregamento Injetora", "Faltou
autorização Injetora para descarga. Aguardando Solicitação de Home";
SetDO doDCMFault1,1;
Stop;
Reset doDCMFault1;

CASE 5:
ErrWrite "Falha Sensor Presença Canal", "Verificar ou
Limpar Sensor / Presença Continua";
SetDO doDCMFault1,1;
Stop;
Reset doDCMFault1;
 CASE 15:
ErrWrite "Falha Posição Home", "Robô está em uma
posição desconhecida após alteração do ponteiro do programa";
Stop;
WaitTime 1;
ENDTEST
ENDPROC

PROC Extração_Injetora()
Injetora;
IF Griper_Carregado=TRUE THEN
Quebra_Bolsas;
Banho;
IF diSerraAuto=1 AND diSerra_Carga=1 THEN
Buffer_Referência;
Serra_Depósito;
Move_to_Home;
ExitCycle;
ELSEIF diSerraAuto=0 THEN
IF Peça_OK_Griper = TRUE THEN
Mesa_Depósito;
Move_to_Home;
ExitCycle;
ELSEIF Peça_NOK_Griper = TRUE THEN
Descarte;
Move_to_Home;
ExitCycle;
ENDIF
ENDIF
Buffer_Depósito;
IF Griper_Carregado=FALSE THEN
 IF diSerraAuto=1 AND diSerra_Desc=1 THEN
Serra_Retirada;
IF Peça_OK_Griper = TRUE THEN
Mesa_Depósito;
IF diSerraAuto=1 AND diSerra_Carga=1 AND
diPresença_Buffer=1 THEN
Buffer_Retirada;
Serra_Depósito;
ENDIF
Move_to_Home;
ExitCycle;
ELSE
Move_to_Home;
Descarte;
IF diSerraAuto=1 AND diSerra_Carga=1 AND
diPresença_Buffer=1 THEN
Buffer_Retirada;
Serra_Depósito;
ENDIF
Move_to_Home;
ExitCycle;
ENDIF
ELSEIF diSerraAuto=0 OR diSerra_Desc=0 THEN
Buffer_Retirada;
IF Peça_OK_Griper = TRUE THEN
Mesa_Depósito;
Move_to_Home;
ExitCycle;
ELSEIF Peça_NOK_Griper = TRUE THEN
Descarte;
Move_to_Home;
 ExitCycle;
ENDIF
ENDIF
ENDIF
ELSEIF Griper_Carregado=FALSE THEN
Move_to_Home;
ExitCycle;
ENDIF
ENDPROC

PROC absj()
MoveAbsJ [[0,0,0,0,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]\NoEOffs, v1000, z50, tool0;
ENDPROC
PROC Carrega_Trap_Seg_Inj()
IDelete Int_Cond_Seg_Injetora;
CONNECT Int_Cond_Seg_Injetora WITH Trap_Seg_Injetora;
WaitUntil Cond_Seg_Injetora = 1;
ISignalDI\SingleSafe, Cond_Seg_Injetora, 0, Int_Cond_Seg_Injetora;
ENDPROC
PROC Descarrega_Trap_Seg_Inj()
IDelete Int_Cond_Seg_Injetora;
ENDPROC
TRAP Trap_Seg_Injetora
WHILE Cond_Seg_Injetora = 0 DO
StopMove;
ErrWrite "Falta Condições de Segurança para Acesso
Injetora","Verificar Injetora em Automático, Máquina e Portas Abertas ou retorno para
Home";
Stop;
IF Cond_Seg_Injetora = 1 StartMove;
ENDWHILE
 Carrega_Trap_Seg_Inj;
ENDTRAP
PROC Lubrifica_Garra()
Incr Cont_Lubri;
IF Cont_Lubri > 35 THEN
MoveJ [[234.17,2217.02,995.25],
[0.370483,0.0237302,0.770269,0.518521],[0,-2,0,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v800, z50, Gripper;
MoveJ [[1929.08,2140.82,789.41],[0.637116,-
0.694045,0.242946,-0.231002],[0,-2,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],
v600, fine, Gripper;
Set do_ev_lub_garra;
WHILE Lubri_Garra <= 2 DO
SetDO do_ev_lub_garra, 1;
Fechar_Griper;
SetDO do_ev_lub_garra, 1;
Abrir_Griper;
Incr Lubri_Garra;
ENDWHILE
SetDO do_ev_lub_garra, 0;
MoveJ [[315.37,2283.43,1012.78],[0.00288582,-0.87976,-
0.475408,0.000711102],[1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v2500,
fine, Gripper;
Clear Lubri_Garra;
Clear Cont_Lubri;
ENDIF
ENDPROC
PROC teste_eixo()
MoveJ [[1413.27,-199.08,1359.99],[0.00404068,-0.925541,0.378624,-
0.000873915],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, fine,
Gripper;
WaitDI diPresença_Buffer, 1;
ENDPROC
ENDMODULE