%%%
  VERSION:1
  LANGUAGE:ENGLISH
%%%
MODULE PIEZAS_2
  CONST robtarget derecha:=[[928.41,-361.69,403.7],[0.013761,0.014155,-0.999648,0.017731],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  CONST robtarget izquierda:=[[1061.18,290.79,403.18],[0.014487,0.015464,-0.999616,0.017888],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  CONST robtarget centro:=[[1023.35,6.79,566.41],[0.01472,0.015963,-0.999601,0.01806],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  CONST robtarget REPOSO:=[[756.73,511.92,1418.18],[0.950014,-0.058237,0.139597,0.27312],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  PERS tooldata pinza:=[TRUE,[[-3.8137,2.78313,105.574],[1,0,0,0]],[0.5,[0,0,0],[1,0,0,0],0,0,0]];
  VAR num dcha:=0;
  VAR num izda:=0;

  PROC main()
    WaitUntil DInput(di1)=1;
    MoveJ REPOSO,v2000,z20,pinza;
    
    IF GInput(gi1)=0 THEN
      CERRAR_PINZA(centro);
    ELSEIF GInput(gi1)=2 THEN
      ABRIR_PINZA(izquierda);
      izda:=1;
    ELSEIF GInput(gi1)=1 THEN
      ABRIR_PINZA(derecha);
      dcha:=1;
    ELSEIF GInput(gi1)=3 AND izda=1 THEN
      CERRAR_PINZA(izquierda);
      REP;
      CENT;
      ABRIR_PINZA(centro);
      izda:=0;
    ELSEIF GInput(gi1)=3 AND dcha=1 THEN
      CERRAR_PINZA(derecha);
      REP;
      CENT;
      ABRIR_PINZA(centro);
      dcha:=0;
    ELSE
      REP;
    ENDIF
  ENDPROC

    PROC REP()
    MoveJ REPOSO,v2000,z20,pinza;
  ENDPROC

  PROC ABRIR_PINZA(robtarget punto)
    MoveJ Offs(punto,0,0,100),v1000,fine,pinza;
    MoveL punto,v1000,fine,pinza;    
    Reset do1;
    WaitTime 1;
    MoveL Offs(punto,0,0,100),v1000,fine,pinza;
  ENDPROC
  PROC CERRAR_PINZA(robtarget punto1)
    MoveJ Offs(punto1,0,0,100),v1000,fine,pinza;
    MoveL punto1,v1000,fine,pinza;    
    Reset do1;
    WaitTime 1;
    MoveL Offs(punto1,0,0,100),v1000,fine,pinza;
  ENDPROC
ENDMODULE
