6 configuración y programación del...

Integración de sistema de visión artificial y robot en aplicación tipo Pick&Place6 Configuración y programación del Robot

6 Configuración y programación del Robot

6.1 Parámetros de configuración del robotPara la configuración del robot se han modificado los parámetros que se detallan a continuación,

indicando el valor asignado y la función.

6.1.1 Parámetros generales● PRGUSR=”UBP” => define el programa contenedor de las definiciones de las variables

globales que se utilizarán

● BZR=0 => desconecta el dichoso pitido cuando se produce una alarma.

6.1.2 Parámetros de comunicaciones● CPRC13=2 => especifica modo Data Link para puerto OPT13

● COMDEV[2]=OPT13 => asigna puerto OPT13 a COM3

6.1.3 Parámetros para la función de tracking● TRMODE=1 => Activa las funciones de tracking.

● EXTENC=(3,0,0,0,0,0) => Asocia M_ENC[0] con el encoder conectado al canal 1 de la tarjeta de encoder en slot 2.

● P_ENCDLT=(?,?,?,0,0,?,0,0)1 => variable de posición que almacena el avance de la banda (en unidades cartesianas del sistema de referencia del robot) por cada pulso de encoder. En este proyecto el valor teórico suponiendo un montaje preciso en la posición que se indica en Ilustración 6.1: Variable de escalado de encoder P_ENCDLT, teniendo en cuenta que la banda avanza 81,68mm/revMOTOR en dirección -Y, y la resolución del encoder es de 4000pulsos/revmotor, resulta

P_ENCDLT=(0,-81.68/4000,0,0,0,0,0,0)=(0,-0.02042,0,0,0,0,0,0)

Para obtener este parámetro de forma empírica se utilizan los programas CLE1 y CLE2, que se describen más adelante. En cualquier caso hay que esperar P_ENCDLT.C=0, incluso asegurarlo por programa, puesto que lo contrario significaría que la cinta tiene rotación respecto del robot, y produciría errores en las composiciones finales de los movimientos.

1 Las coordenadas A y B son necesariamente 0 para el SCARA RH5, además de L1 y L2 puesto que no hay ejes adicionales interpolados en este proyecto.

Ingeniería en Automática y Electrónica Industrial 6-1


● TRCWDST=20 => Distancia en mm mínima para considerar dos posiciones diferentes en el buffer de tracking.

6.2 Listado y descripción de los programas escritos para el robotA continuación se listan los programas que se han escrito en el robot, las funciones que realiza y

la forma de ejecución prevista.

● MAIN: programa principal que se debe arrancar desde el panel de operación (por tanto se ejecuta en SLOT 1) para poner en marcha el sistema. Este programa accede al buffer de posiciones leidas y realiza los movimientos para la manipulación de las piezas, utilizando las funciones de tracking del robot.

● COM: programa de comunicaciones y gestión del buffer de posiciones leidas. Es arrancado por MAIN y ejecutado en SLOT 2, gestiona las comunicaciones con el PC mediante el servidor TCP/IP, convierte las posiciones leidas al sistema de referencia del robot, las ordena y las almacena en el buffer.

● CLE1,CLE2: programas para la calibración del encoder de banda, obteniendo P_ENCDLT. Se deben ejecutar secuencialmente desde el panel de operación, según se detalla en el capítulo de calibración de encoder.

● CLV1,CLV2,CLV3,CLV4: programas para la calibración de la posición relativa del sistema de visión artifical.Se deben ejecutar secuencialmente desde el panel de operación, según se


Ilustración 6.1: Variable de escalado de encoder P_ENCDLT

Encoder: 4000pul/rev

Avance de banda: 81,68mm/revMOTOR

P_ENCDLT:(0,-0.02042,0,0,0,0,0,0)


detalla en el capítulo de calibración de visión.

● UBP: programa para declaración de variables globales, nunca se ejecuta.

● TTR: programa para test de la función de tracking, cuando se arranca desde el panel de operación el robot queda solidario con la banda, si se mueve incluso manualmente el robot debe seguirla. Sirve para confirmar que los parámetros de configuración de la función de tracking están correctamente.

6.3 Implementación de la función de trackingEn el programa MAIN se implementa la manipulación de las piezas detectadas, utilizando el

seguimiento de encoder que proporciona la instrucción TRK. Como se explicó en el capítulo descriptivo del robot, cuando se activa el modo tracking con TRK ON PTRK MTRK, el robot se mueve solidario con la banda y actualiza continuamente al valor actual de la posición PTRK que llamaremos PTRKactual, escalando con P_ENCDLT:

PTRKactual=PTRK+(M_ENCactual – MTRK) * P_ENCDLT

Además la posición PTRKactual la utilizará como sistema de referencia para las instrucciones de movimiento que se ejecuten durante el modo tracking, es decir, si se ejecutan las líneas que se muestran en Texto 6.1: Instrucciones de modo tracking, la operación PTRKactual*POBJ es la composición relativa de la posición POBJ respecto de la posición PTRKactual, por tanto si POBJ son las coordenadas de posición de la pieza respecto del origen del sistema de visión artificial, y PTRKactual es ese origen, el robot se moverá hacia la pieza objetivo siguiendo su movimiento en la cinta.

En lo sucesivo se denominarán P_ORGV≡PTRK (Posición ORiGen de sistema de visión) la posición de referencia de tracking y PV≡POBJ (Posición respecto Visión).

Hay que tener en cuenta también que la composición P_ORGVactual*PV la realiza el robot en el sistema de coordenadas de herramienta, es decir, considerando que P_ORGVactual es la posición del extremo útil de la herramienta, y si está girado la composición se hace conforme a ese giro. De esta forma se simplifica la operación en caso de modificar la herramienta o de trabajar con varias a la vez, puesto que todas las operaciones siguen siendo válidas simplemente modificando P_TOOL.

Si los sistemas de referencia tienen la misma orientación, hay que asegurarse de que P_ORGV.A=P_ORGV.B=P_ORGV.C=0 y poco más, porque si por ejemplo P_ORGV.C<>02 el robot realiza una rotación de las coordenadas XOBJ e YOBJ según ese ángulo P_ORGV.C.2 La composición se hace concretamente sobre P_ORGVactual, pero puesto que la banda se mueve en línea recta, o

sea, se desplaza sin rotar respecto del robot, necesariamente P_ENCDLT.C=0 por tanto durante todo el recorrido P_ORGVactual.C=P_ORGV.C.


TRK ON PTRK MTRK ' Activa modo tracking

MOV POBJ ' Mueve a PTRKactual*POBJ

TRK OFF ' Fin de modo tracking

Texto 6.1: Instrucciones de modo tracking


Para considerar la rotación del sistema caben 2 posibilidades3:

1. Registrar esa rotación en la variable P_ORGV, en las componentes de rotación A,B,C. Por ejemplo en el caso de Ilustración 6.2: Rotación simple de sistema de visión respecto delrobot, donde dada la posición de la cámara determina un sistema de coordenadas rotado respecto del robot en torno al eje Y, por tanto debería ser P_ORGV.B=180. Sin embargo esta forma es bastante compleja de modelar si hay más de una rotación, y la calibración empírica tampoco es simple.

2. Mantener P_ORGV=(P_ORGV.X,P_ORGV.Y,P_ORGV.Z,0,0,0,0,0) y procesar la rotación con funciones matemáticas en el robot. En general para obtener una posición PB=(xB,yB,zB) respecto de un sistema B, que está rotado respecto de un sistema A, se construye una matriz de rotación donde cada fila es un vector unitario del sistema A respecto de B, de la forma siguiente:

En este caso la cámara ya fija la situación relativa de los ejes X e Y, por tanto con el ángulo β que se muestra en x la orientación está totalmente determinada, y la matriz de rotación se simplifica como se muestra en Ilustración 6.4: Matriz de rotación particularizada.

En este proyecto se resuelve la orientación por este segundo método, de modo que el robot cuando recibe las posiciones del sistema de visión las transforma con las líneas de programa de Texto 6.2: Sentencias para la rotación en el programa del robot.

Durante el proceso de instalación y puesta en marcha del sistema será necesario por tanto determinar el origen del sistema de visión, que se almacenará en la variable global de usuario

3 Considerando siempre que el robot será el encargado de procesar la información y realizar la calibración necesaria, de modo que el sistema de visión se limite a enviar las posiciones halladas (convertidas eso sí de pixel a mm.).


Ilustración 6.3: Matriz de rotación genérica

Ilustración 6.2: Rotación simple de sistema de visión respecto del robot

Y+

X+Y+

X+Sistema Visión Sistema Robot


P_ORGV, así como el ángulo β que se almacena en M_RVBETA. Ambos se obtienen mediante los programas de calibración CLV1,CLV2,CLV3,CLV4.

Por último falta indicar que la coordenada PV.C se puede utilizar para dar a la pinza una orientación determinada, medida respecto del sistema del robot puesto que P_ORGV.C=0. Si se quiere orientar la pieza en base a la posición angular que detecte el sistema de visión, entonces debe referirse a éste sumando el ángulo β según se muestra en Ilustración 6.5: Rotación y orientación depieza, con PV.C = θR = β + θV (si el sentido positivo de ángulo de visión coincide con el robot) .

En caso de tener una aplicación sin visión artificial, únicamente con un detector de barrera para


Ilustración 6.4: Matriz de rotación particularizada

140 PROBOT=P_ZERO

150 PROBOT.X=COS(M_RVBETA)*VX+SIN(M_RVBETA)*VY ' ROTACION del sistema de vision respecto del robot

160 PROBOT.Y=SIN(M_RVBETA)*VX-COS(M_RVBETA)*VY ' ROTACION del sistema de vision respecto del robot

Texto 6.2: Sentencias para la rotación en el programa del robot

Ilustración 6.5: Rotación y orientación de pieza

θR

Y+

X+

Y+

X+

Sistema VisiónSistema Robot

β

θV


capturar el paso del producto, la operación se simplificaría y sólo sería necesario conocer la posición del objeto en el momento de la activación del sensor (con un proceso de calibración correpondiente). Esta posición sería P_ORGV, y el seguimiento se haría haciendo un movimiento a P_ZERO, puesto que la posición relativa es nula:

El robot debe almacenar en buffer el valor de encoder (MTRK) para cada detección de producto. En este caso la orientación deseada de la herramienta sería más sencillo almacenarla en P_ORGV.C, es decir, registrarla en el proceso de calibración.

6.4 Procedimiento de calibración del encoder de bandaPara obtener P_ENCDLT mediante un procedimiento empìrico una vez instalado el sistema

completo, se realiza un procedimiento que se denomina calibración de encoder banda, y los programas CLE1.prg y CLE2.prg se utilizan para tal fin. Los pasos a realizar son los siguientes:

1. Colocar una pieza en la zona de trabajo del robot, mejor hacia un extremo, y llevar la herramienta del robot hasta situarlo sobre la pieza. Esto se puede hacer mediante la consola de mano trabajando en modo JOG (con SERVO ON), o bien por tratarse de un SCARA con los motores parados (SERVO OFF) empujando el brazo robot, ya que los ejes J1,J2 y J4 no tienen freno, y el eje J3 (vertical) que si tienen freno se puede liberar con el pulsador J3 BRAKE RELEASE. Ejecutar el programa CLE1.prg, que registrará la posición y valor de encoder en las variables globales P_CLEA (PA) y M_CLEA (MENCA) respectivamente.

2. A continuación avanzar la banda manualmente, con cuidado de que la pieza no deslice, hasta situarla hacia el otro extremo del rango del robot.

3. Llevar el robot hasta la pieza igual que en el paso 1. La orientación no es importante mantenerla puesto que de todas formas las componentes A,B y C se pondrán a 0 por programa (las componentes A y B no intervienen en el SCARA, pero se hace genéricamente). Si no se hace así y durante el movimiento manual se mueve alguna orientación, después se reflejará este movimiento durante la operación de tracking.



MOV P_ZERO ' Mueve a PTRKactual


Texto 6.3: Sentencias para tracking sin visión, 1ª forma

TRBASE PTRK ' Sistema de referencia general


MOV PTRK ' Mueve a PTRKactual


Texto 6.4: Sentencias para tracking sin visión, 2ª forma



150 MENCDIF#=M_CLEB-M_CLEA ' MENCDIF# es el avance del encoder

160 ' Corrección de MENCDIF#

165 RANGO#=M_ENCMAX-M_ENCMIN ' Fondo de escala del contador de encoder

170 IF MENCDIF#<(-RANGO#/2) THEN MENCDIF#= MENCDIF# + RANGO# + M_ENCMIN ' Corrección movimiento decremental encoder

171 IF MENCDIF#>(RANGO#/2) THEN MENCDIF#=MENCDIF#-RANGO#-M_ENCMIN ' Corrección movimiento incremental encoder

180 '

190 P_ENCDLT=(P_CLEB-P_CLEA)/(MENCDIF#) ' Calculo de P_ENCDLT

195 P_ENCDLT.C=0 ' Si al pasar de A a B se ha cambiado la orientación de la herramienta, se desprecia

Texto 6.5: Sentencias para obtener P_ENCDLT en el robot

Ilustración 6.6: Procedimiento de calibrado de encoder. Obtención de P_ENCDLT

PA, MENCA

PB, MENCB

1

2

3

4


4. Ejecutar el programa CLE2.prg, que registrará la posición y valor de encoder en las variables globales P_CLEB (PB) y M_CLEB (MENCB) respectivamente, y calculará P_ENCDLT.

Todas las variables utilizadas antes son globales y por tanto retentivas, de modo que el programa CLE2 tiene acceso a las variables obtenidas por CLE1. También pueden consultarse a posteriori incluso tras desconectar el robot para conprobar el resultado de la calibración. Las líneas de programa para el cálculo de P_ENCDLT se muestran en Texto 6.5: Sentencias para obtenerP_ENCDLT en el robot.

Es necesario contemplar una posible correción del avance del encoder, porque el contador M_ENC es cíclico y si se desborda por M_ENCMIN (defecto 0) toma el valor M_ENCMAX (defecto 1.000.000.000), y viceversa. Puede ocurrir entonces que si se conecta alimentación al robot para hacer la calibración del encoder de banda, con lo cual M_ENC=0 (esta variable no es retentiva), y la banda se mueve un poco en el sentido decremental del encoder, la variable M_CLEA tome un valor muy alto (por ejemplo M_CLEA=999.999.999 si se mueve 1 pulso), y si se avanza por ejemplo 1000 pulsos de encoder en sentido incremental, se tiene

MENCDIF = M_CLEB-M_CLEA = 999 – 999.999.999 = -999.999.000 !!

Sin embargo al aplicar la corrección:

MENCDIF = -999.999.000 + 1.000.000.000 = 1000 ☺

De igual modo ocurriría si la calibración se hiciera en el sentido decremental de la banda, partiendo de M_ENC=0, entonces:

MENCDIF = 999.999.999 – 0 – 1.000.000.000 = -1000 ☺

Observar que el sentido de avance en el proceso de la calibración puede ser cualquiera, lo que no puede ocurrir es que se tengan medidas fuera de un rango del contador de encoder.

6.5 Procedimiento de calibración del sistema de referencia del equipo de visión artificial

6.5.1 Transformación entre Sistema de Visión y Sistema de Robot Tal y como se ha concluido en el capítulo sobre las funciones de tracking del robot, es necesario

para la operación de la aplicación determinar la posición relativa del sistema de referencia con que trabaja el equipo de visión artificial, respecto del sistema de referencia del robot. Expresando la posición del objeto con coordenadas extendidas[ROBMM], la traslación y rotación se concentran en la matriz de transformación de Ilustración 6.7: Matriz de transformación completa genérica.

En nuestra aplicación, la disposición relativa de ambos sistema se muestra en Ilustración 6.9:Sistemas de referencia en una posición arbitraria de banda y equipo de visión4. La matriz de transformación se simplifica entonces, puesto que la posición relativa de los ejes X e Y está fijada por la cámara de visión, y el eje Z será coincidente porque practicamente cámara y robot trabajan en el mismo plano5. Por tanto toda la rotación se recoge en el ángulo M_RVBETA. La traslación a su

4 En este capítulo el tratamiento de las piezas se hace considerándolas puntuales, y su orientación por tanto no se considera. Los ángulos βV (orientación vista por el sistema de visión) y βV (orientación respecto del robot para ajustar la coordenada C de la herramienta) se tratan en el capítulo sobre las funciones de tracking.

5 La banda puede tener un desplazamiento vertical durante su recorrido que tampoco afecta a esta consideración,



vez se recogerá en P_ORGV, resultando la expresión de Ilustración 6.8: Matriz de transformacióncompleta particularizada.

El proceso de calibración servirá para obtener las variables P_ORGV y M_RVBETA de forma empírica, una vez instalado el sistema, al igual que se hace al obtener P_ENCDLT, que de hecho debe haberse obtenido previamente puesto que se utilizará en este procedimiento.

En primer lugar se obtiene P_ORGV calibrando sobre un punto P1A situado en el origen del sistema de visión, y después se obtiene βRV a partir de otro punto P2A situado en el campo de visión

con ambas coordenadas no nulas.

puesto que en la variable P_ENCDLT.Z de calibración del encoder de banda ya se recoge. Hay que considerar que el plano sobre el que se sitúa la pieza en la captura de la imagen, se mantiene durante la manipulación del robot puesto que la banda es plana, luego si la ventosa de succión puede absorber esa inclinación, recogerá la pieza en la posición vista por la cámara sin problemas.


Ilustración 6.7: Matriz de transformación completa genérica

Ilustración 6.8: Matriz de transformación completa particularizada


6.5.2 Obtención de P_ORGV (traslación entre sistemas de referencia)Si el robot tiene a su alcance el campo de visión, para obtener P_ORGV basta situar una pieza en

el origen del sistema, y llevar el robot hasta esa pieza. Pero normalmente no es el caso y el campo de visión está fuera del rango del robot, por tanto será necesario llevar esa pieza de referencia hasta donde el robot pueda alcanzarla, y medir el avance del encoder de banda para restar la cantidad de movimiento correspondiente en cada coordenada, según P_ENCDLT. El proceso se ilustra en Ilustración 6.10: Procedimiento de calibrado de origen de visión. Obtención de P_ORGV.Los pasos a realizar son los siguientes:


Ilustración 6.9: Sistemas de referencia en una posición arbitraria de banda y equipo de visión

βR

Y+

X+Y+

X+

Sistema Robot

βV

X+

V0 (P_ORGV)

(M_RVBETA)ΒRV


1. Colocar una pieza centrada en el origen del campo de visión. Para ello poner la cámara en modo VIDEO y utilizar una pieza circular marcada para que pueda situarse facilmente en la esquina superior izquierda, que es donde está el origen del sistema de visión. Una vez situada en la imagen sólo debe verse ¼ de la pieza, lógicamente. Ejecutar el programa CLV1.prg y entonces el PC registrará el valor de encoder en la variable global M_CLV1A (MENC1A).

2. A continuación avanzar la banda manualmente, con cuidado de que la pieza no deslice, hasta situarla en el rango del robot.

3. Llevar el robot hasta la pieza. La orientación no es importante mantenerla puesto que de todas formas las componentes A,B y C se pondrán a 0 por programa (las componentes A y


180 MENCDIF#=M_CLV1B-M_CLV1A ' MENCDIF es el avance del encoder

190 P_CLV1A=P_CLV1B-P_ENCDLT*MENCDIF# ' Mediante P_ENCDLT se obtiene la posicion A respecto al robot

200 P_ORGV=P_CLV1A ' Final

Texto 6.6: Sentencias para obtener P_ORGV en el robot

Ilustración 6.10: Procedimiento de calibrado de origen de visión. Obtención de P_ORGV


B no intervienen en el SCARA, pero se hace genéricamente). Si no se hace así la composición final de movimientos no será correcta..

4. Ejecutar el programa CLV2.prg, que registrará la posición y valor de encoder en las variables globales P_CLV1B (P1B) y M_CLV1B (MENC1B) respectivamente, y calculará P_ORGV.

Todas las variables utilizadas antes son globales y por tanto retentivas, de modo que el programa CLV2 tiene acceso a las variables obtenidas por CLV1. También pueden consultarse a posteriori incluso tras desconectar el robot para conprobar el resultado de la calibración. Las líneas de programa para el cálculo de P_ORGV son las de Texto 6.6: Sentencias para obtener P_ORGV en elrobot.

6.5.3 Obtención de βRV (rotación entre sistemas de referencia)El procedimiento es similar al anterior, pero ahora la pieza se debe situar en un punto del campo

de visión donde las coordenadas sean ambas no nulas, para no tener argumentos nulos en la función atan2 (ver Ilustración 6.11: Procedimiento de calibrado de rotación de visión. Obtención de βRV.). Lo ideal es alejar lo más posible la pieza del origen, para reducir los errores del cálculo de posición del sistema de visión, al relativizarlos con medidas mayores. Los pasos a realizar son los siguientes:

1. Colocar una pieza en el campo de visión. Una vez situada en la imagen ejecutar el programa CLV3.prg, que se queda a la espera de la posición vista desde el PC. En el PC ejecutar


Ilustración 6.11: Procedimiento de calibrado de rotación de visión. Obtención de βRV.


“Enviar X Y”, y entonces el PC registrará esa posición y el valor de encoder en las variables globales P_VA y M_CLV2A (MENC2A).

2. A continuación avanzar la banda manualmente, con cuidado de que la pieza no deslice, hasta situarla en el rango del robot.

3. Llevar el robot hasta la pieza. La orientación no es importante mantenerla puesto que de todas formas la componente C se pondrá a 0 por programa (las componentes A y B no intervienen en el SCARA).

4. Ejecutar el programa CLV4.prg, que registrará la posición y valor de encoder en las variables globales P_CLV2B (P2B) y M_CLV2B (MENC2B) respectivamente, y calculará P_ORGV.

Todas las variables utilizadas antes son globales y por tanto retentivas, de modo que el programa CLV4 tiene acceso a las variables obtenidas por CLV3. También pueden consultarse a posteriori incluso tras desconectar el robot para conprobar el resultado de la calibración.

Las líneas de programa para el cálculo de M_RVBETA son las de Texto 6.7: Sentencias paraobtener M_RVBETA en el robot.


180 MENCDIF#=M_CLV2B-M_CLV2A ' MENCDIF es el avance del encoder

190 P_CLV2A=P_CLV2B-P_ENCDLT*MENCDIF# ' Mediante P_ENCDLT se obtiene la posicion A respecto al robot

200 XREL=P_CLV2A.X-P_ORGV.X ' Posicion X relativa

210 YREL=P_CLV2A.Y-P_ORGV.Y ' Posicion Y relativa

215 AUXHOR=XREL*P_VA.X-YREL*P_VA.Y

216 AUXVER=XREL*P_VA.Y+YREL*P_VA.X

220 M_RVBETA=ATN2(AUXVER,AUXHOR) ' Se obtiene la rotación del sistema de visión respecto del sistema del robot

230 M_RVBETG=DEG(M_RVBETA) ' Valor en grados para monitorización

Texto 6.7: Sentencias para obtener M_RVBETA en el robot


6.6 Ejecución multitarea de programas de manipulación y comunicaciones

Después de haber realizado los procedimientos de calibración el sistema está operativo, y se puede poner en marcha el programa MAIN para la ejecución automática de la manipulación de


Ilustración 6.12: Coordinación entre programas de manipulación y comunicaciones

Carga y arrancaslot 2 : COM

Extrae registro depila de tracking

Manipula pieza

Espera comandodel PC

Recibido“PCTrigger”

?

Captura valorde encoder M_ENC

Espera comandodel PC

RecibidoDatosPosicion

?

Guarda Datos yencoder en Buffer

si procede

Recibido“PCTrigger”

?

ROBOTSLOT1: MAIN.prg

ROBOTSLOT2: COM.prg

Espera que hayadatos en pila de

tracking

Espera que lapieza esté en

rango alcanzable PILA TRACKING

(FIFO)

Ordena posicionessegún situaciónde robot y cinta


piezas. Este programa a su vez arranca a COM, y ambos se ejecutan paralelamente:

● COM se comunica con el PC y después de procesar los datos almacena en el buffer de tracking las posiciones de las piezas a manipular

● MAIN está pendiente del buffer y va sacando los datos. Cuando una posición está a su alcance va a cogerla y la suelta en la posición prevista.

El planteamiento es robusto puesto que los programas son independientes, y el sincronismo temporal entre ambos no es crítico, en cuanto que no importa en que momento COM ha guardado una posición en el buffer, puesto que MAIN la recogerá junto con un valor de encoder asociado que le permite localizar la posición actual, en base al avance medido de encoder. Este valor está en la variable global M_ENC y por tanto ambos programas pueden acceder a su lectura. El diagrama de flujo de esta secuencia se ilustra en Ilustración 6.12: Coordinación entre programas demanipulación y comunicaciones.

6.7 Estructura de subrutinas del programa de comunicacionesEl programa COM se ha escrito basado en subrutinas, de modo que cuando espera una orden del


Ilustración 6.13: Diagrama de subrutinas utilizadas en COM

Espera trama del PC, con formato genérico:INPUT CORDEN$,CNCICLOS$,VX,VY,VTHETA,NPIEZA,NPZAS

CORDEN$=”PCTrigger” Subrutina *TRIGGER

ROBOTSLOT2: COM.prg

CORDEN$=”Datos”

CORDEN$=”Cero”

Subrutina *DATOS

Subrutina *CERO

Subrutina *ORDENA


PC, en función de la orden recibida realiza una función determinada. Puesto que la secuencia de órdenes no tiene por qué siempre ser la misma, de esta forma se garantiza que el programa no se quedará descontrolado si el PC envía las distintas tramas en un orden no esperado. Además se utiliza una única instrucción INPUT con un formato de instrucción común para todas las órdenes, aunque no se utilicen algunos “argumentos”, de esta forma si hay una descoordinación en la secuencia de órdenes el robot no queda en estado de error por recibir datos en “Illegal Format”.

6.8 Criterio de ordenación de las posiciones detectadasEl sistema de visión artificial ordena las piezas localizadas en función del sentido de barrido de la

imagen, sin embargo para el robot es más interesante seguir un orden de manipulación en función de la dirección de avance de la cinta, porque de lo contrario alguna pieza puede ir alejándose y entonces el tiempo necesario para cogerla se alarga, perdiendo efectividad el proceso de pick&place. El ordenado de las piezas lo hace el robot en el programa COM, utilizando el vector P_ENCDLT que al fin y al cabo es un vector en la dirección de avance de la cinta. Para medir este avance se utiliza el producto escalar del vector posición de la pieza, después de hacerle la rotación necesaria para referirlo al sistema de referencia del robot (RPV1 y RPV2 en Ilustración 6.14: Criteriopara ordenar las piezas detectadas según el avance en la banda), con el vector P_ENCDLT, resultando el valor de proyección de uno sobre otro, multiplicado por el módulo de P_ENCDLT:


Ilustración 6.14: Criterio para ordenar las piezas detectadas según el avance en la banda

Y+

X+P_ENCDLT

Sistema Robot

P_ORGV

RPV1

RPV2


Las posiciones cuando se reciben se almacenan en un array temporal PARRAY(20) y se extrae el avance de cada una en MAVANCE(20) mediante la expresión:

MAVANCE(N)=P_ENCDLT.X*PARRAY(N).X+P_ENCDLT.Y*PARRAY(N).Y

Posteriormente se recorre un número de veces suficiente el vector PARRAY(20) comparando el avance de cada posición con la consecutiva, y permutando ambas si procede, como se muestra en Texto 6.8: Bucle de ordenado de posiciones en el robot.


560 FOR VUELTAS=1 TO NPZAS ' Vueltas para asegurar que todas las posiciones se mueven como deben, creo que sobra una

570 FOR M=1 TO NPZAS-1 ' Recorrido del array hasta penúltimo580 N=M+1590 IF MAVANCE(M)<MAVANCE(N) THEN 600 PTEMP=PARRAY(M)605 MTEMP=MAVANCE(M)610 PARRAY(M)=PARRAY(N)620 MAVANCE(M)=MAVANCE(N)630 PARRAY(N)=PTEMP640 MAVANCE(N)=MTEMP650 ENDIF660 NEXT670 NEXT672 FOR N=1 TO NPZAS ' Solo queda enviar el array ordenado al

buffer de tracking673 PAUX=PARRAY(N)674 TRWRT PAUX,MCAP# ' La posicion (x,y) y el valor de

encoder asociado a la captura se guardan en buffer676 NEXTTexto 6.8: Bucle de ordenado de posiciones en el robot

Texto 6.9: Representación gráfica del orden de manipulación de las piezas detectadas

AVANCE1=∥ PV 1R ∥⋅cos1=

PV 1R ⋅P ENCDLT∥P ENCDLT∥

AVANCE 2=∥ PV 2R ∥⋅cos2=

PV 2R ⋅P ENCDLT∥P ENCDLT∥

6 configuración y programación del...

Documents