Practica2_4_PaletizadoRobotstudio

MODULE Module1
    CONST robtarget pHome:=[[336.99,-306.03,467.74],[0.00681109,-0.361444,-0.932365,-0.00264041],[-1,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
    CONST robtarget p0:=[[455.21,1.06,355.07],[0.00730495,-1.70149E-5,-0.999973,-1.24297E-7],[-1,-1,0,1],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
    CONST robtarget p10:=[[40.16,-416.59,333.57],[0.0133112,-0.684158,-0.729201,-0.00407804],[-1,0,-1,1],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];

    VAR robtarget p100;
    VAR intnum sel := 0;
    VAR BOOL flag := FALSE;

    !***********************************************************
    !
    ! Módulo:  Module1
    !
    ! Descripción:
    !   <Introduzca la descripción aquí>
    !
    ! Autor: perez
    !
    ! Versión: 1.0
    !
    !***********************************************************


    !***********************************************************
    !
    ! Procedimiento Main
    !
    !   Este es el punto de entrada de su programa
    !
    !***********************************************************
    PROC main()

        !Cancelamos la interrupción para poder realizar un proceso continuo
        IDelete sel;
        !Conectamos la rutina trap con el selector
        CONNECT sel WITH rutina_selector;
        ISignalDI SEL_FUNC, 0, sel;
        ISleep sel;
        !El procés comença EN posició inicial
        MoveJ pHome, v1000, fine, Pinça;


        IF SEL_FUNC = 1 THEN

            StartMove;
            Principal;

        ELSEIF SEL_FUNC = 0 THEN

            intermittence;

        ELSE

        ENDIF


    ENDPROC

    PROC Principal()

            ! El robot espera EN home hasta que reciba señal de palet vacío
             MoveJ pHome, v1000, fine, Pinça;
             IWatch sel;
             WHILE REARME = 0 DO

                 Reset RBT_ENFUNC;
                 intermittence;

                 IF REARME = 1 THEN

                     GOTO inici_sequencia;

                 ENDIF

             ENDWHILE

             inici_sequencia:
             Reset RBT_ENESPERA;
             SET RBT_ENFUNC;

             ! Comienza la secuencia
                P100 := P10;
                flag := FALSE;

                FOR i FROM 0 TO 1 DO   ! Bucle para hacer la paletización EN dos niveles

                    FOR j FROM 1 TO 2 DO  !Bucle para hacer dos partes de un cuadrado

                        FOR x FROM 1 TO 2 DO !Bucle para hacer dos cuadrados dentro de un cuadrado

                        Reset OPEN_GRIPP;
                        MoveJ pHome, v1000, z150, Pinça;
                        WaitDI OK_POS, 1;

                        !El robot se dirige a cojer la pieza EN la posición p0
                        MoveJ p0, v1000, fine, Pinça;
                        SET OPEN_GRIPP;
                        MoveL Offs(p0,0,0,-50), v1000, fine, Pinça;
                        Reset OPEN_GRIPP;
                        SET CLOSE_GRIPP;

                        !El robot se dirige a dejar la pieza EN la posición de paletizado, pasando por pHome
                        MoveJ pHome, v1000, z150, Pinça;
                        MoveJ p100, v1000, fine, Pinça;
                        MoveL Offs(p100,0,0,-50), v1000, fine, Pinça;
                        Reset CLOSE_GRIPP;
                        SET OPEN_GRIPP;
                        MoveL Offs(p100,0,0,0), v1000, fine, Pinça;

                        !Modifiquem la posició p100 per la próxima iteració
                        p100 := Offs (p100, -70, 0, 0);
                        ENDFOR


                    !Cambiamos valor de p100 para hacer la otra parte del cuadrado

                    IF flag = FALSE THEN

                        p100 := p10;
                        p100 := Offs(p100, 0, -70, 0);

                    ELSEIF flag = TRUE THEN
                        P100 := P10;
                        p100 := Offs(p100, 0, -70, 60);

                    ELSE

                    ENDIF

                   ENDFOR


                   !Cambiamos valor de p100 para hacer la planta superior del paletizado
                    flag := TRUE;
                    p100 := p10;
                    p100 := Offs(p100, 0, 0, 60) ;

                ENDFOR

                P100 := P10;
                SET PALET_PLE;


    ENDPROC

    PROC intermittence()

       Reset RBT_ENFUNC;
       PulseDO\PLength:=1, RBT_ENESPERA  ;
       WaitTime 1;

    ENDPROC

    TRAP rutina_selector

           StopMove;
           Reset RBT_ENFUNC;
           SET RBT_ENESPERA;

           WHILE SEL_FUNC = 0 DO
               intermittence;
               IF SEL_FUNC = 1 THEN
                   GOTO final_trap;

               ENDIF

           ENDWHILE

           final_trap:
           SET RBT_ENFUNC;
           StartMove;

    ENDTRAP

ENDMODULE