%%% VERSION:1 LANGUAGE:ENGLISH %%% MODULE discos_SOFAMA !

AUTHOR: Guillermo Luis Oviedo - Zero_G Electronics Consulting - Teodelina 201 1 ! Posicin del pos1 CONST pos pos_pos1:=[725.000000,0.000000,712.000000]; !origen CONST orient ori_pos1:=[0.382683,0.000000,0.923880,0.000000];!girar vi endo el trebol CONST confdata conf_pos1:=[0,0,0,0]; CONST extjoint ejes_pos1:=[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]; CONST robtarget rob_pos1:=[pos_pos1,ori_pos1,conf_pos1,ejes_pos1]; PERS tooldata grip :=[TRUE,[[0,0,0],[1,0,0,0]],[5,[9,0,9],[1,0,0,0],0. 01,0.01,0.01]]; !valores falsos ver Tecnica !posiciones de inicios de movimientos Modificar con modpos CONST robtarget pos1 :=[[33.1,-61.8,54.9], [0.34,-0.42,0.42,-0.72], [1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pos2 :=[[33.1,-61.8,64.9], [0.34,-0.42,0.42,-0.72], [1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pos3 :=[[-27.2,53.4,-15.1], [0.27,0.32,-0.22,0.56], [1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST ROBTARGET pos4 :=[[-27.2,53.4,-25.1], [0.27,0.32,-0.22,0.56], [1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; ! Posicin de approx pos1 CONST pos pos_approxpos1:=[725.000000,0.000000,812.000000]; CONST robtarget rob_approxpos1:=[pos_approxpos1,ori_pos1,conf_pos1,eje s_pos1]; PERS tooldata herramienta:=[FALSE,[[97,0,223],[0.924,0,0,0.383,0]],[5, [-23,0,75],[1,0,0,0],0,0,0]]; PERS loaddata carga:=[5,[50,0,50],[1,0,0,0],0,0,0]; CONST dionum listo:=1; PROC main() abrir_grip; tomar_disco; ENDPROC ! Procedimiento principal

PROC abrir_grip() WHILE TRUE DO MoveJ A,v100,fine,grip; WaitDI econtrol,listo; tomar_disco; MoveL B1,v80,z5,grip; MoveJ C1,v100,z5,grip; MoveL C,v80,fine,grip; abrir_grip; MoveL C1,v80,z5,grip; ! movimiento rapido hasta un punto proximo al trebol MoveL pos1, v1000, z50, grip; ! descenso hacia el disco con velocidad lenta y alta precision MoveL pos2, v50, z3, grip; ! activacion de la grip

Set do15; ! espera de 0.5 segundos para asegurar accion grip WaitTime 0.5; ! movimiento rapido hasta un punto proximo al trebol MoveL pos3, v1000, z50, grip; ! descenso hacia el trebol con velocidad lenta y alta precision MoveL pos4, v50, z3, grip; ENDWHILE ENDPROC PROC Ir_posicion_esp() MoveJ conf_espera,VMAX,z30,herramienta; !Mover a posicin inicial ENDPROC ! Procedimiento para abrir grip PROC Abrir() Set abrirgrip; DInput(di1) = 1; Doutput(do3) = 0; num1 < num2 THEN nombre1 = nombre2; DOutput(do1) = 0 AND pos1.x > 100; Reset cerrargrip; ENDPROC PROC tomar() Set grip; l digital grip WaitTime 0.3; GripLoad carga; est tomada ENDPROC PROC Dejar() Reset grip; WaitTime 0.3; gripload load0; tomada endproc proc ir_posicion_esp() icin de espera while dinput(terminar)=0 do nar if dinput(disco_defectuosa)=1 then !esperar la seal de dis co defectuosa setdo activar_alarma,0; tomar_disco; disco setdo activar_alarma,1; ar seal de alarma endif endwhile endproc ! procedimiento para cerrar gr ip proc cerrar() Set cerrargrip; WaitTime 0.5; Reset cerrargrip; !activ !parar alarma !agarra el !esperar la seal de termi !mover a pos !Abrir grip !Esperar 0,3 segundos !sealar que no hay disco !Esperar 0,3 segundos !Sealar que el disco !Cerrar grip activando la sea

ENDPROC PROC tomar_disco() MoveJ *,VMAX,z60,herramienta; !Mov. con poca precisin MoveL *,V500,z20,0.herramienta; !Mov. Lnea recta con precisin MoveL *,V150,FINE,herramienta; !Bajar con precisin mxima !aca poner el sensor para que interrumpa la instruccion anterior tomar; !tomar el disco MoveL *,V200,z20,herramienta !Subir con el disco tomado ENDPROC PROC Dejar_disco() !poner contador para descontar altura d la pila MoveJ *,VMAX,z30,herramienta !Mover hacia almacn discos salida MoveJ *,V300,z30,herramienta Dejar; !Dejar el disco en la pila ENDPROC ENDMODULE !aca empezar a llevar el disco al torno incluirlo como subR entre tomar() y dej ar()