Publicidad
'==================
START
*DATA,T2,33
START
*MOVEMENT,T3,34
*MAIN:
DELAY
5
GOTO
*MAIN
'==================
'Programa de rececpcion de datos del FQ
'Recibe el numero de piezas y el valor de encoder en ASCII
'==================
*DATA:
SEND
ETH
TO
BFU_CNT$
SEND
ETH
TO
BFU_ENC$
'==================
'Conversion de ASCII a decimal
'==================
COUNT%=VAL(BFU_CNT$)
'==================
'Adquisicion de las posiciones
'==================
IF
COUNT%>0
THEN
'
'Lee las posiciones del FQ-M
FOR
I%=1
TO
COUNT%
SEND
ETH
TO
NEXT
I%
'
FOR
I%=1
TO
COUNT%
'Inserta la posicion de la pieza en la ultima posicion
'de la cola del contador 1
CADDQUE
1,P[I%]
NEXT
I%
ENDIF
GOTO
*DATA
'
'==================
'Programa de movimiento del robot
'==================
*MOVEMENT:
'Borra los elementos de la cola que se han salido del rango de operacion
WHILE
CCHKQUE(1) = -1
CRMVQUE
1
WEND
'
'Comprueba que la pieza esta dentro del area de trabajo del robot
IF
CCHKQUE(1)>0
THEN
IF
CCHKQUE(1)=1
THEN
'Si esta dentro se posiciona encima y se mueve a su misma velocidad
CTMOVE
1,Z=002.0
IF
(CCHKQUE(1)=2)
'Cambia la posicion del eje Z par aque pueda coger la pieza
CTDRIVE(016.5)
DELAY
100
'D0 y D1 son las salidas digitales cableadas a la bomba de vacio
'Pone en ON la bomba de vacio
DO(21)=1
DO(20)=1
CTDRIVE(008.0)
P[I%]
THEN'Comprueba que se este realizando el seguimiento
47
Publicidad
