Anexo2 Programas del Robot

Integración de sistema de visión artificial y robot en aplicación tipo Pick&Place
A2 Listado de programas del robot
A2 Listado de programas del robot
A2.1 Programa MAIN.prg
10 OVRD 100
20 ACCEL 50,50
25 CNT 1
30 'LOADSET 1,1
40 'OADL ON
50 '
55 TRCLR
56 '
60 IF M_PSA(2)=0 THEN GOTO *SLOT2RUN
70 XLOAD 2,"COM"
' Carga programa COM es SLOT 2
80 WAIT C_PRG(2)="COM"
' Espera que el programa COM este cargado en SLOT 2
90 XRUN 2
' Arranca programa en SLOT 2
100 *SLOT2RUN
110 'IF M_PSA(3)=0 THEN GOTO *SLOT3RUN
120 ' XLOAD 3,"ROB2"
130 ' WAIT C_PRG(3)="ROB2"
140 'XRUN 3
150 '*SLOT3RUN
160
WHILE(1)
170
DATABUF=M_TRBFCT
' Para monitorizacion rapida en Variables de Program
Monitor
180
WAIT M_TRBFCT>0
190
GOSUB *TRACKING
200 WEND
210 *TRACKING
220 TRRD PV,MV#
' Extrae de Buffer posicion respecto de P_PRGV y encoder en
el momento de la foto
230 ESPERA=1
240 WHILE ESPERA=1
250 PORGVYA=TRWCUR(1,P_ORGV,MV#) ' Origen de sistema de referencia actualizado
segun encoder
255 PACTUAL=PORGVYA*PV ' Posicion de pieza actualizada segun encoder
260 IF (POSCQ(PACTUAL))=1 THEN ESPERA=0 ' La pieza entra en zona de trabajo
270 WEND
280 'DLY 1
300 TRK ON,P_ORGV,MV#
' Inicia TRACKING con origen de referencia P_ORGV
310 MOV PV,+100
' Mueve hacia PACTUAL*P_ZERO/P_ZERO*PV=PACTUAL*PV
315 CNT 0
320 MOV PV,-10
325 M_OUT(4)=1
330 MOV PV,+100
' Sube un poco
340 TEMP#=P_ENCDLT.X ' para comprobar resolucion de decimales
345 CNT 1
360 TRK OFF
370 MOV PSUELTA
371 MOV PSUELTA,-15
375 M_OUT(4)=0
390 RETURN
PV=(+50.338,-69.844,+0.000,+0.000,+0.000,+0.000)(0,0)
PORGVYA=(+445.920,+186.979,+154.392,+0.000,+0.000,+0.000,+0.000,+0.000)(4,0)
P_ORGV=(+571.037,+468.513,+157.575,+0.000,+0.000,+0.000,+0.000,+0.000)(4,0)
Ingeniería en Automática y Electrónica Industrial
A2-1
Integración de sistema de visión artificial y robot en aplicación tipo Pick&Place
A2 Listado de programas del robot
PACTUAL=(+496.258,+117.135,+154.392,+0.000,+0.000,+0.000,+0.000,+0.000)(4,0)
PSUELTA=(+395.579,+262.056,+214.300,+0.000,+0.000,+33.629,+0.000,+0.000)(4,0)
PESPERA=(+329.030,+411.254,+229.303,+0.000,+0.000,+34.507,+0.000,+0.000)(4,0)
A2.2 Programa COM.prg
10 '############################################################################
20 ' COMUNICACION CON EL SISTEMA DE VISION ARTIFICAL
30 ' ROTACION DE COORDENADAS DEL SISTEMA DE VISION RESPECTO DEL ROBOT
40 ' ORDENADO Y ALMACENAMIENTO EN BUFFER PARA TRACKING
50 '############################################################################
55 DIM PARRAY(20) ' Array para ordenar las posiciones recibidas, no se esperan
mas de 10 o 12
56 DIM MAVANCE(20) 'Array de valores de avance, criterio utilizados para ordenar
las piezas
60 '
70 ' Al inicio el Robot debe tener el Buffer vacío, ya que al terminar un
programa se cierran los puertos abiertos en el mismo
80 M_E_CORR=300
90 '
100 OPEN "COM3:" AS #1 ' COMDEV[2]=OPT13 (COM3=OPT13); NETPORT[3]=10003 (Puerto
asignado); CPRCE13=2 (modo DataLink)
110 '
120 *WAITCOM
130 INPUT #1,CORDEN$,CNCICLOS$,VX,VY,VTHETA,NPIEZA,NPZAS ' Espera comando del
PC
140 IF CORDEN$="PCTrigger" THEN GOSUB *TRIGGER ' Procesamiento de PCTrigger
150 IF CORDEN$="Datos" THEN GOSUB *DATOS ' Procesamiento de datos recibidos
160 IF CORDEN$="Cero" THEN GOSUB *CERO ' Procesamiento cero, se almacena en
buffer M_ENC y posición=(0,0), para test
170 GOTO *WAITCOM
180 '
190 '
200 *TRIGGER
210 NTRIG=NTRIG+1 ' Contador para depuracion
220 MCAP#=M_ENC-M_E_CORR ' Captura encoder, con corrección por software si
procede
230 CRESP$="ACKPCTrigger"+CNCICLOS$ ' Respuesta compuesta con el nº de ciclo
para que PC confirme sincronismo comunicaciones
240 PRINT #1,CRESP$ ' Confirma al PC la recepción y procesamiento de "PCTrigger"
250 RETURN
260 '
270 '
280 *DATOS
290 NDAT=NDAT+1 ' Contador para depuracion
300 PROBOT=P_ZERO
310 PROBOT.X=COS(M_RVBETA)*VX+SIN(M_RVBETA)*VY ' ROTACION del sistema de vision
respecto del robot
320 PROBOT.Y=SIN(M_RVBETA)*VX-COS(M_RVBETA)*VY ' ROTACION del sistema de vision
respecto del robot
325 PARRAY(NPIEZA)=PROBOT ' Guarda posiciones en array. NPIEZA viene numerado
desde 1, justamente como trabaja el robot
340 IF NPIEZA=NPZAS THEN ' El robot confirma recepcion del paquete cuando recibe
la ultima pieza
350
PRINT #1,"ACKDatos" ' Confirma al PC la recepcion correcta de datos
355
GOSUB *ORDENA
360 ENDIF
370 RETURN
Ingeniería en Automática y Electrónica Industrial
A2-2
Integración de sistema de visión artificial y robot en aplicación tipo Pick&Place
A2 Listado de programas del robot
380 '
390 '
400 *CERO
410 NCER=NCER+1 ' Contador para depuracion
420 PROBOT=P_ZERO
430 PROBOT.X=0
440 PROBOT.Y=0
450 TRWRT PROBOT,M_ENC
460 RETURN
470 '
500 *ORDENA
510 IF NPZAS=1 THEN
520
TRWRT PROBOT,MCAP# ' La posicion (x,y) y el valor de encoder asociado a
la captura se guardan en buffer
530 ENDIF
550 IF NPZAS>1 AND NPZAS<30 THEN ' Segunda condicion es de seguridad
552
FOR N=1 TO NPZAS ' calcula las variables de medida utilizadas para
ordenar
554
MAVANCE(N)=P_ENCDLT.X*PARRAY(N).X+P_ENCDLT.Y*PARRAY(N).Y
556
NEXT
560
FOR VUELTAS=1 TO NPZAS ' Vueltas para segurar que todas las posiciones
se mueven como deben, creo que sobra una
570
FOR M=1 TO NPZAS-1 ' Recorrido del array hasta el penúltimo
580
N=M+1
590
IF MAVANCE(M)<MAVANCE(N) THEN ' A
permutaaaaaaaaaaaaaaaaarrrrrrrr
600
PTEMP=PARRAY(M)
605
MTEMP=MAVANCE(M)
610
PARRAY(M)=PARRAY(N)
620
MAVANCE(M)=MAVANCE(N)
630
PARRAY(N)=PTEMP
640
MAVANCE(N)=MTEMP
650
ENDIF
660
NEXT
670
NEXT
672
FOR N=1 TO NPZAS ' Solo queda enviar el array ordenado al buffer de
tracking
673
PAUX=PARRAY(N)
674
TRWRT PAUX,MCAP# ' La posicion (x,y) y el valor de encoder asociado
a la captura se guardan en buffer
676
NEXT
680 ENDIF
690 RETURN
PARRAY(1)=(+21.128,-134.564,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(2)=(+50.338,-69.844,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(3)=(+124.382,-72.575,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(4)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(5)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(6)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(7)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(8)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(9)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(10)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(11)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(12)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(13)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(14)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(15)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
Ingeniería en Automática y Electrónica Industrial
A2-3
Integración de sistema de visión artificial y robot en aplicación tipo Pick&Place
A2 Listado de programas del robot
PARRAY(16)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(17)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(18)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(19)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PARRAY(20)=(+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PROBOT=(+21.128,-134.564,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PTEMP=(+50.338,-69.844,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
PAUX=(+50.338,-69.844,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
A2.3 Programa CLE1.prg
10
'###############################################################################
##
20 ' CALIBRACION DEL ENCODER DE BANDA :
PASO 1
30 ' 1) CLE1.prg: Posición A para obtener P_ENCDLT
40 ' 2) CLE2.prg: Posición B para obtener P_ENCDLT
50
'###############################################################################
##
60 '
70 '
Con la banda parada, colocar una pieza en la misma y llevar el Robot
hasta situarlo en la pieza
80 IF M_CLEST<>0 AND M_CLEST<>2 THEN ERROR 9100 ' Control de estado para
asegurar el orden correcto de los pasos
85 '
90 M_CLEA=M_ENC 'Captura lectura de encoder en posición A
100 P_CLEA=P_FBC 'Captura posicion A de robot en base a feedback de servos
120 '
140 M_CLEST=1 ' Paso 1 realizado
220 HLT
P_CLEA=(+534.251,+117.527,+153.202,+0.000,+0.000,-6.596,+0.000,+0.000)(4,0)
P30=(-0.003,-0.039,+0.000,+0.000,+0.000,+0.000)(4,0)
P31=(-0.003,-0.039,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)
PA=(+347.140,+318.769,+158.405,+0.000,+0.000,+0.099)(4,0)
PB=(+309.082,-118.517,+155.235,+0.000,+0.000,+0.004)(4,0)
A2.4 Programa CLE2.prg
10
'###############################################################################
##
20 ' CALIBRACION DEL ENCODER DE BANDA :
PASO 2
30 ' 1) CLE1.prg: Posici¢n A para obtener P_ENCDLT
40 ' 2) CLE2.prg: Posici¢n B para obtener P_ENCDLT
50
'###############################################################################
##
60 '
70 ' Previamente se debe haber realizado el paso 1 (CLE1.prg)
80 ' Desplazar la banda, llevar el robot hasta la pieza, y ejecutar este
programa
90 IF M_CLEST<>1 THEN ERROR 9100 ' Control de estado para asegurar el orden
correcto de los pasos
100 '
110 '
120 M_CLEB=M_ENC 'Captura lectura de encoder en posicion B
130 P_CLEB=P_FBC 'Captura posicion B de robot en base a feedback de servos
Ingeniería en Automática y Electrónica Industrial
A2-4
Integración de sistema de visión artificial y robot en aplicación tipo Pick&Place
A2 Listado de programas del robot
140 '
150 MENCDIF#=M_CLEB-M_CLEA ' MENCDIF# es el avance del encoder
160 ' Corrección de MENCDIF#, aunque lo mejor es mover la banda al arranque para
alejarse del límite del rango
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
200 '
210 M_CLEST=2 ' Paso 2 y último realizado
220 HLT
P_CLEB=(+330.230,-341.555,+148.012,+0.000,+0.000,-34.542,+0.000,+0.000)(4,0)
P_CLEA=(+534.251,+117.527,+153.202,+0.000,+0.000,-6.596,+0.000,+0.000)(4,0)
P30=(-0.003,-0.039,+0.000,+0.000,+0.000,+0.000)(4,0)
P31=(-0.003,-0.039,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)
PA=(+347.140,+318.769,+158.405,+0.000,+0.000,+0.099)(4,0)
PB=(+309.082,-118.517,+155.235,+0.000,+0.000,+0.004)(4,0)
A2.5 Programa CLV1.prg
10 '##########################################################################
20 ' CALIBRACION DE POSICION DEL SISTEMA VISION :
PASO 1
30 ' 1) CLV1.prg: Posicion A para obtener P_ORGV (origen de sistema de vision)
40 ' 2) CLV2.prg: Posicion B para obtener P_ORGV (origen de sistema de vision)
50 ' 3) CLV3.prg: Posicion A para obtener M_RVBETA (rotacion de sistema de
vision)
60 ' 4) CLV4.prg: Posicion B para obtener M_RVBETA (rotacion de sistema de
vision)
70 ' IMPORTANTE: previamente debe haberse calibrado encoder para tener P_ENCDLT
80 '##########################################################################
90 '
100 ' Con la banda parada, Colocar pieza en el ORIGEN (arriba izquierda) del
campo de vision y ejecutar este programa
130 '
140 M_CLV1A=M_ENC
' Captura lectura de encoder en posicion A
145 '
150 M_CLVST=1
' Paso 1 realizado
160 HLT
A2.6 Programa CLV2.prg
10 '##########################################################################
20 ' CALIBRACION DE POSICION DEL SISTEMA VISION :
PASO 1
30 ' 1) CLV1.prg: Posicion A para obtener P_ORGV (origen de sistema de vision)
40 ' 2) CLV2.prg: Posicion B para obtener P_ORGV (origen de sistema de vision)
50 ' 3) CLV3.prg: Posicion A para obtener M_RVBETA (rotacion de sistema de
vision)
60 ' 4) CLV4.prg: Posicion B para obtener M_RVBETA (rotacion de sistema de
vision)
70 ' IMPORTANTE: previamente debe haberse calibrado encoder para tener P_ENCDLT
80 '##########################################################################
90 '
100 ' Previamente se debe haber realizado el paso 1 (CLV1.prg)
Ingeniería en Automática y Electrónica Industrial
A2-5
Integración de sistema de visión artificial y robot en aplicación tipo Pick&Place
A2 Listado de programas del robot
110 '
Desplazar la banda, llevar el robot a la pieza con la orientacion
deseada, y ejecutar este programa
120 '
130 IF M_CLVST<>1 THEN ERROR 9100 ' Control de estado para segurar el orden de
los pasos correctos
140 '
150 M_CLV1B=M_ENC
' Captura lectura de encoder en posicion B
160 P_CLV1B=P_FBC
' Captura posicion B de robot en base a feedback de
servos
170 '
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
' Finalmente se obtiene el origen del sistema de
vision, para utilizarlo con TRK
210 P_ORGV.A=0
220 P_ORGV.B=0
230 P_ORGV.C=0 ' Orientaciones nulas puesto que P_ORGV será el sistema de
refencia relativo durante tracking
240 '
250 M_CLVST=2
' Paso 2 realizado
260 HLT
P_CLV1B=(+239.501,-277.500,+149.140,+0.000,+0.000,-14.506,+0.000,+0.000)(4,0)
P_CLV1A=(+571.037,+468.513,+157.575,+0.000,+0.000,-14.506,+0.000,+0.000)(4,0)
P_ORGV=(+571.037,+468.513,+157.575,+0.000,+0.000,+0.000,+0.000,+0.000)(4,0)
A2.7 Programa CLV3.prg
10 '##########################################################################
20 ' CALIBRACION DE POSICION DEL SISTEMA VISION :
PASO 3
30 ' 1) CLV1.prg: Posicion A para obtener P_ORGV (origen de sistema de vision)
40 ' 2) CLV2.prg: Posicion B para obtener P_ORGV (origen de sistema de vision)
50 ' 3) CLV3.prg: Posicion A para obtener M_RVBETA (rotacion de sistema de
vision)
60 ' 4) CLV4.prg: Posicion B para obtener M_RVBETA (rotacion de sistema de
vision)
70 ' IMPORTANTE: previamente debe haberse calibrado encoder para tener P_ENCDLT
80 '##########################################################################
90 '
110 '
Poner una pieza en el campo de vision del robot, ejecutar este
programa, y desde PC "Enviar (X,Y) al robot"
140 '
150 OPEN "COM3:" AS #1
160 INPUT #1,VXA,VYA
' Respecto de V (vision), recibe posiciones X e Y, de A
170 M_CLV2A=M_ENC
' Captura lectura de encoder en posicion A
175 P_VA=P_ZERO
176 P_VA.X=VXA
177 P_VA.Y=VYA
180 '
190 M_CLVST=3
' Paso 3 realizado
200 HLT
P_VA=(+167.000,+125.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
A2.8 Programa CLV4.prg
10 '##########################################################################
20 ' CALIBRACION DE POSICION DEL SISTEMA VISION :
PASO 1
30 ' 1) CLV1.prg: Posicion A para obtener P_ORGV (origen de sistema de vision)
Ingeniería en Automática y Electrónica Industrial
A2-6
Integración de sistema de visión artificial y robot en aplicación tipo Pick&Place
A2 Listado de programas del robot
40 ' 2) CLV2.prg: Posicion B para obtener P_ORGV (origen de sistema de vision)
50 ' 3) CLV3.prg: Posicion A para obtener M_RVBETA (rotacion de sistema de
vision)
60 ' 4) CLV4.prg: Posicion B para obtener M_RVBETA (rotacion de sistema de
vision)
70 ' IMPORTANTE: previamente debe haberse calibrado encoder para tener P_ENCDLT
80 '##########################################################################
90 '
100 ' Previamente se debe haber realizado el paso 3 (CLV3.prg)
110 '
Desplazar la banda, llevar el robot a la pieza con cualquier
orientacion, y ejecutar este porograma
120 '
130 IF M_CLVST<>3 THEN ERROR 9100 ' Control de estado para segurar el orden de
los pasos correctos
140 '
150 M_CLV2B=M_ENC
' Captura lectura de encoder en posicion B
160 P_CLV2B=P_FBC
' Captura posicion B de robot en base a feedback de
servos
170 '
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 obtienen 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
240 '
250 M_CLVST=4
' Paso 4 realizado
260 HLT
P_CLV2B=(+418.417,-284.882,+149.513,+0.000,+0.000,-32.846)(4,0)
P_CLV2A=(+673.448,+288.980,+156.002,+0.000,+0.000,-32.846,+0.000,+0.000)(4,0)
P_ORGV=(+571.037,+468.513,+157.575,+0.000,+0.000,+0.000,+0.000,+0.000)(4,0)
P_VA=(+167.000,+125.000,+0.000,+0.000,+0.000,+0.000,+0.000,+0.000)(0,0)
Ingeniería en Automática y Electrónica Industrial
A2-7