
FUNCTION_BLOCK FB36

VAR_TEMP
    tempCalc: DINT; // variable temporaire pour le calcul de position
END_VAR
VAR
    step: ARRAY[0..24] OF BOOL;
    TR: ARRAY[0..26] OF BOOL;
    posX: ARRAY[0..3] OF DINT; //stock valeur temporire axe X pour calibration
    posZ: ARRAY[0..3] OF DINT; //stock valeur temporaire axe Z pour calibration
    i: INT;
    flag_DetecteurPresence: BOOL;
    
    //position pour début de teach
    posRefZ: INT:= 2800; // variable pour l'axe Z lors du teatch 
    posRefX: INT:= 25; // variable pour l'axe X lors du teatch
    
    //position relative et non absolue pr rapport au coin scanné
    posX13: INT:= -109; // pos relative axe X
    posX16: INT:= 4132; // pos relative axe X
    posZ13: INT:= 641; // pos relative axe Z
    deriveZ16: INT:= 0; // dérive relative vertical entre Z13 et Z16
    posZ9: INT:= -535; // pos relative axe Z
    deriveZ12: INT:= -2; // dérive relative vertical entre Z9 et Z12
    posZ5: INT:= -1721; // pos relative axe Z
    deriveZ8: INT:= -20; // dérive relative vertical entre Z5 et Z8
    posZ1: INT:= -2937; // pos relative axe Z
    deriveZ4: INT:= -5; // dérive relative vertical entre Z1 et Z4
    posX17: INT:= 5907; // position relative de la prise de rack en X
    posZ17: INT:= -2960; // position relative de la prise de rack en Z

   

END_VAR
    
    //transition
    TR[0]:= step[0] AND ("HMI_DB".HMI_Z10_learnPos AND "G7_GEMA_DB".step[4]
                        OR "HMI_DB".HMI_Z10_learnPos AND "G7_GEMA_DB".step[3] AND NOT "Global".UpperPosition
                        OR "G7_GEMA_DB".step[3] AND ("PosListX".X[17] = 0 OR "PosListZ".Z[17] = 0)
                        OR "G7_GEMA_DB".step[5]); // 0 vers 1
    TR[1]:= step[1] AND "G7_ResetPos_DB".step[5]; // 1 vers 2
    TR[2]:= step[2] AND X_Move_End AND Z_Move_End; // 2 ves 3
    TR[3]:= step[3] AND detecteurPresenceRack AND Z_Jog_End; // 3 vers 4
    TR[4]:= step[4] AND not Z_Jog_End; // 4 vers 5
    TR[5]:= step[5] AND Z_Jog_End; // 5 vers 6
    TR[6]:= step[6] AND i<4 AND NOT Z_jog_end; // 6 vers 7
    TR[7]:= step[7] AND NOT detecteurPresenceRack AND Z_Jog_End; // 7 vers 8
    TR[8]:= step[8] AND NOT Z_Jog_End; // 8 vers 9
    TR[9]:= step[9] AND Z_Jog_End; // 9 vers 10
    TR[10]:= step[10] AND NOT Z_Jog_End; // 10 vers 3
    TR[11]:= step[6] AND i >3 AND NOT Z_Jog_End; // 6 vers 11
    TR[12]:= step[11]; // 11 vers 12
    TR[13]:= step[12] AND X_Move_End AND Z_Move_End; // 12 ves 13
    TR[14]:= step[13] AND detecteurPresenceRack AND X_Jog_End; // 13 vers 14
    TR[15]:= step[14] AND not X_Jog_End; // 14 vers 15
    TR[16]:= step[15] AND X_Jog_End; // 15 vers 16
    TR[17]:= step[16] AND i<4 AND NOT X_Jog_end; // 16 vers 17
    TR[18]:= step[17] AND NOT detecteurPresenceRack AND X_Jog_End; // 17 vers 18
    TR[19]:= step[18] AND NOT X_Jog_End; // 18 vers 19
    TR[20]:= step[19] AND X_Jog_End; // 19 vers 20
    TR[21]:= step[20] AND NOT X_Jog_End; // 20 vers 13
    TR[22]:= step[16] AND i >3 AND NOT X_Jog_End; // 16 vers 21
    TR[23]:= step[21]; // 21 vers 22
    TR[24]:= step[22]; // 22 vers 23
    TR[25]:= step[23] AND X_Move_End AND Z_Move_End; // 23 vers 4
    TR[26]:= step[24] AND ("G7_GEMA_DB".step[4] OR "G7_GEMA_DB".step[3] OR "G7_GEMA_DB".step[6]); // 24 vers 0
    
    //etape
    IF TR[0] THEN // 0 vers 1
        step[0]:= false;
        step[1]:= true;
    END_IF;
    IF TR[1] THEN // 1 vers 2
        step[1]:= false;
        step[2]:= true;
    END_IF;
    IF TR[2] THEN // 2 ves 3
        step[2]:= false;
        step[3]:= true;
    END_IF;
    IF TR[3] THEN // 3 vers 4
        step[3]:= false;
        step[4]:= true;
    END_IF;
    IF TR[4] THEN // 4 vers 5
        step[4]:= false;
        step[5]:= true;
    END_IF;
    IF TR[5] THEN // 5 vers 6
        step[5]:= false;
        step[6]:= true;
    END_IF;
    IF TR[6] THEN // 6 vers 7
        step[6]:= false;
        step[7]:= true;
    END_IF;
    IF TR[7] THEN // 7 vers 8
        step[7]:= false;
        step[8]:= true;
    END_IF;
    IF TR[8] THEN // 8 vers 9
        step[8]:= false;
        step[9]:= true;
    END_IF;
    IF TR[9] THEN // 9 vers 10
        step[9]:= false;
        step[10]:= true;
    END_IF;
    IF TR[10] THEN // 10 vers 3
        step[10]:= false;
        step[3]:= true;
    END_IF;
    IF TR[11] THEN // 6 vers 11
        step[6]:= false;
        step[11]:= true;
    END_IF;
    IF TR[12] THEN // 11 vers 12
        step[11]:= false;
        step[12]:= true;
    END_IF;
    IF TR[13] THEN // 12 vers 13
        step[12]:= false;
        step[13]:= true;
    END_IF;
    IF TR[14] THEN // 13 vers 14
        step[13]:= false;
        step[14]:= true;
    END_IF;
    IF TR[15] THEN // 14 vers 15
        step[14]:= false;
        step[15]:= true;
    END_IF;
    IF TR[16] THEN // 15 vers 16
        step[15]:= false;
        step[16]:= true;
    END_IF;
    IF TR[17] THEN // 16 vers 17
        step[16]:= false;
        step[17]:= true;
    END_IF;
    IF TR[18] THEN // 17 vers 18
        step[17]:= false;
        step[18]:= true;
    END_IF;
    IF TR[19] THEN // 18 vers 19
        step[18]:= false;
        step[19]:= true;
    END_IF;
    IF TR[20] THEN // 19 vers 20
        step[19]:= false;
        step[20]:= true;
    END_IF;
    IF TR[21] THEN // 20 vers 13
        step[20]:= false;
        step[13]:= true;
    END_IF;
    IF TR[22] THEN // 16 vers 21
        step[16]:= false;
        step[21]:= true;
    END_IF;
    IF TR[23] THEN // 21 vers 22
        step[21]:= false;
        step[22]:= true;
    END_IF;
    IF TR[24] THEN // 22 vers 23
        step[22]:= false;
        step[23]:= true;
    END_IF;
    IF TR[25] THEN // 23 vers 24
        step[23]:= false;
        step[24]:= true;
    END_IF;
    IF TR[26] THEN // 24 vers 0
        step[24]:= false;
        step[0]:= true;
    END_IF;

    
    
        
    //gestion des opérations
    IF step[1] THEN
        "PosListX".X[45]:= posRefX;
        "PosListZ".Z[45]:= posRefZ;
    END_IF;
    IF step[1] OR step[12] THEN
        i:=0;
    END_IF;
    IF step[2] THEN
        Place_X:= INT_TO_BYTE(45);
        Place_Z:= INT_TO_BYTE(45);
    END_IF;
    IF TR[3] THEN
        posZ[i]:= "Global".Position_Z;
        i:= i+1; // attention ordre
    END_IF;
    IF step[11] THEN
        "PosListZ".Z[45]:= (posZ[0] + posZ[1] + posZ[2] + posZ[3])/4;
        "PosListZ".Z[45]:= "PosListZ".Z[45]-65;
        "PosListX".X[45]:= posRefX- 300;
    END_IF;
    IF TR[14] THEN
        posX[i]:= "Global".Position_X;
        i:= i+1; // attention ordre
    END_IF;
    IF step[21] THEN
        "PosListX".X[45]:= (posX[0] + posX[1] + posX[2] + posX[3])/4;
        "PosListZ".Z[45]:= "PosListZ".Z[45]+65;
    END_IF;
    IF step[22] THEN
        //position X à la vertical de 13
        tempCalc:= "PosListX".X[45]+ posX13;
        "PosListX".X[1]:= tempCalc;
        "PosListX".X[5]:= tempCalc;
        "PosListX".X[9]:= tempCalc;
        "PosListX".X[13]:= tempCalc;
        //position à la vertical de 14
        tempCalc:= posX16 - posX13;
        tempCalc:= tempCalc/3;
        tempCalc:= "PosListX".X[45] + posX13 + tempCalc;
        "PosListX".X[2]:= tempCalc;
        "PosListX".X[6]:= tempCalc;
        "PosListX".X[10]:= tempCalc;
        "PosListX".X[14]:= tempCalc;
        //position à la vertical de 15
        tempCalc:= posX16 - posX13;
        tempCalc:= (tempCalc*2)/3;
        tempCalc:= "PosListX".X[45] + posX13 + tempCalc;
        "PosListX".X[3]:= tempCalc;
        "PosListX".X[7]:= tempCalc;
        "PosListX".X[11]:= tempCalc;
        "PosListX".X[15]:= tempCalc;
        //position à la vertical de 16     
        tempCalc:= "PosListX".X[45] + posX16 ;
        "PosListX".X[4]:= tempCalc;
        "PosListX".X[8]:= tempCalc;
        "PosListX".X[12]:= tempCalc;
        "PosListX".X[16]:= tempCalc;

        //position à l'horizontal de 13
        tempCalc:= "PosListZ".Z[45] + posZ13;
        "PosListZ".Z[13]:=tempCalc;
        "PosListZ".Z[14]:=tempCalc +(deriveZ16/3);
        "PosListZ".Z[15]:=tempCalc +(deriveZ16*2/3);
        "PosListZ".Z[16]:=tempCalc +(deriveZ16);
        //position à l'horizontal de 9
        tempCalc:= "PosListZ".Z[45] + posZ9;
        "PosListZ".Z[9]:=tempCalc;
        "PosListZ".Z[10]:=tempCalc +(deriveZ12/3);
        "PosListZ".Z[11]:=tempCalc +(deriveZ12*2/3);
        "PosListZ".Z[12]:=tempCalc +(deriveZ12);
        //position à l'horizontal de 5
        tempCalc:= "PosListZ".Z[45] + posZ5;
        "PosListZ".Z[5]:=tempCalc;
        "PosListZ".Z[6]:=tempCalc +(deriveZ8/3);
        "PosListZ".Z[7]:=tempCalc +(deriveZ8*2/3);
        "PosListZ".Z[8]:=tempCalc +(deriveZ8);
        //position à l'horizontal de 1
        tempCalc:= "PosListZ".Z[45] + posZ1;
        "PosListZ".Z[1]:=tempCalc;
        "PosListZ".Z[2]:=tempCalc +(deriveZ4/3);
        "PosListZ".Z[3]:=tempCalc +(deriveZ4*2/3);
        "PosListZ".Z[4]:=tempCalc +(deriveZ4);
        
        //position prise Z8
        "PosListX".X[17]:= "PosListX".X[45] + posX17;
        "PosListZ".Z[17]:= "PosListZ".Z[45] + posZ17;
        
        
    END_IF;
    IF step[23] THEN
        Place_X:= 9;
        Place_Z:= 9;
    END_IF;
         
    //gestion front
    flag_DetecteurPresence:= detecteurPresenceRack;

END_FUNCTION_BLOCK
