RAPID:
MODULE Module1
CONST jointtarget Home:=[[0,0,0,0,90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Target_10:=[[-350,290,377.774306289],[0,0,1,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Target_20:=[[-350,290,20.000306289],[0,0,1,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Target_30:=[[-350,290,377.774306289],[0,0,1,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Target_40:=[[-350,290,20.000306289],[0,0,1,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Target_50:=[[345.499987542,500,710],[-0.000000016,0.707106802,-0.70710676,-0.000000016],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Target_60:=[[237.829989745,500.000001329,20.393838285],[0.000000047,0.707106802,-0.70710676,-0.000000047],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PROC main()
MoveAbsJ Home,v1000,fine,tool0\WObj:=wobj0;
SetDO CINTA_1,0;
SetDO CINTA_2,0;
SetDO CINTA_3,0;
SetDO Ventosa_ON,0;
SetDO Ventosa_OFF,1;
Pedir_Pieza;
Cinta_A;
Cinta_S;
Cinta_B;
Cinta_S;
MoveAbsJ Home,v1000,fine,tool0\WObj:=wobj0;
Pedir_Pieza;
ENDPROC
PROC Coger()
SetDO Ventosa_OFF,1;
SetDO Ventosa_OFF,0;
SetDO Ventosa_ON,1;
WaitDI Sensor_Pieza,1;
ENDPROC
PROC Soltar()
SetDO Ventosa_ON,0;
SetDO Ventosa_OFF,1;
WaitDI Sensor_Pieza,0;
ENDPROC
PROC Pedir_Pieza()
SetDO CINTA_1,1;
SetDO CINTA_2,1;
WaitDI PIEZA_1,1;
WaitDI PIEZA_2,1;
SetDO CINTA_1,0;
SetDO CINTA_2,0;
ENDPROC
PROC Cinta_A()
MoveJ Target_10,v1000,fine,tVacuum\WObj:=W_Cinta_A;
MoveL Target_20,v1000,fine,tVacuum\WObj:=W_Cinta_A;
Coger;
MoveL Target_10,v1000,fine,tVacuum\WObj:=W_Cinta_A;
ENDPROC
PROC Cinta_B()
MoveJ Target_30,v1000,fine,tVacuum\WObj:=W_Cinta_B;
MoveL Target_40,v1000,fine,tVacuum\WObj:=W_Cinta_B;
Coger;
MoveL Target_30,v1000,fine,tVacuum\WObj:=W_Cinta_B;
ENDPROC
PROC Cinta_S()
MoveJ Target_50,v1000,fine,tVacuum\WObj:=W_Cinta_S;
MoveL Target_60,v1000,fine,tVacuum\WObj:=W_Cinta_S;
Soltar;
MoveL Target_50,v1000,fine,tVacuum\WObj:=W_Cinta_S;
SetDO CINTA_3,1;
ENDPROC
ENDMODULE
