Documentación AGV

Hardware AGV

En esta wiki se hará la documentación correspondiente al hardware del AGV desarrollado en el grupo de investigación CIII.

AGV se corresponde con las siglas de Automatic Guided Vehicle, o lo que es lo mismo, vehículos de guiado automático. Los sistemas de AGVs tienen sus comienzos en 1953, cuando se pensó en hacer realidad un sueño un camión remolque sin conductor. Este primer vehículo precisaba de cable que enterrado en el suelo de la fábrica creaba un campo magnético que servía de guía al vehículo. Poco a poco, se han ido incorporando al mundo industrial a la vez que crecían en aplicaciones y sofisticación.

Al hardware del AGV se lo puede dividir en los siguientes sistemas; el sistema de Alimentación, el de Movimiento y el de Control. Estos sistemas serán explicados en los capítulos siguientes. También se mostrarán unos programas de ejemplo en lenguaje python para realizar pruebas con el AGV.

Sistema de Alimentación

La alimentación del AGV se genera desde un banco de 2 baterías de gel de12V conectadas en serie, lo que nos daría un total de 24V. Los componentes que se deben alimentar son los 2 motores ( tracción y dirección ) y el control de la llave H. Estas alimentaciones son de 24V y 12V respectivamente.

Sistema de Movimiento

El AGV posee 2 sistemas de movimiento con control independiente y distinta funcionalidad. Uno es usado para realizar el movimiento de traslación del AGV y el otro para direccionar al mismo. Cada uno está compuesto por 1 motor y un encoder, el cual nos permite saber la posición del motor y con ella realizar diversos calculos ( distancia recorrida, velocidad, aceleración, etc ). Cada motor está controlado por una llave H configurable, la cual nos permite trabajar en varios modos de funcionamiento. Estos modos son posición, velocidad y frenado; se explican más adelante. Según el movimiento que debe realizar el motor, este debe ser usado en alguno de los modos disponibles.

Tracción

Posee un motor de 112V de alimentación nominal, pero en este caso se la hace funcionar con una alimentación de 24V. Este motor se puede utilizar en cualquiera de los modos disponibles.

Dirección

Posee un motor de 24V nominales, al cual se lo alimenta con este voltaje. Este motor esta conectado con el eje de dirección del AGV a través de una correa dentada y un engranaje, y es el que permite que el AGV realice movimientos de giro. Este motor se debe utilizar en el modo posición.

Sistema de Control

Es el encargado de controlar ambos motores y al AGV en general. A este sistema lo dividimos lógicamente en el procesador, la llave H y el control PID; aunque físicamente está implementados en una misma placa de circuito impreso. El AGV posee 2 de estas placas las cuales controlan por separado a cada uno de los motores o sistemas de movimiento.

Procesador

El controlador principal es un PIC16F870 el cual se encarga de comunicar la placa con el exterior, realizar el procesado de señales para aplicar el control PID al motor y generar el PWM que controlará la conmutación de los transistores de la llave H.

Comunicación

La comunicación principal de la placa se realiza a través de RS232 al cual se le agregó un pequeño protocolo. También tiene disponibles físicamente otros puertos ( CAN, RJ45 ), pero estos no están programados en software. Para comunicarse por RS232 con la placa se debe usar la siguiente configuración:

Configuración

8 Bits, sin paridad, 1 bit de parada y 9600bps.

Protocolo

La llave se comunica con el exterior a través del puerto serie con un pequeño protocolo, el cual permite la lectura del estado y la configuración de la misma. Podemos dividir el protocolo en 2 tramas principales. Ambas tramas están compuestas por una cadena con las siguientes especificaciones.

Trama de Lectura

La trama de lectura esta formada por un string de 8 carácteres, donde el inicial y el final deben ser asteriscos "*", el segundo carácter debe ser un arroba '@', el tercero el registro que deseamos leer y luego valores sin importancia.

En la tabla anterior R representa un ASCII y las X son valores sin importancia.

Trama de Escritura

La trama de escritura está compuesta por un string de 7 carácteres donde el inicial y final también deben ser asteriscos "*", el segundo representa el registro y luego el valor que le queremos escribir teniendo en cuenta que el primer caracter es el de menos peso. En este caso sería el tercero de la cadena.

En la tabla anterior V identifica los valores de configuración que se desea configurar en el registro.

Registros

La siguiente tabla muestra todos los registros de la llave H.

Llave H

La llave H es la encargada de entregar la energía a los motores. Su funcionamiento está controlado por el procesador y tiene ciertos parámetros configurables. Según los valores de estos parámetros podemos distinguir 3 modos de funcionamiento. Estos modos se explican a continuación.

Modos de Funcionamiento

Modo Posición

Este modo nos permite configurar el valor de la posición final del motor. Sería el recorrido espacial que tendrá el motor. Para utilizarla debemos configurar la llave H en el modo POSICIÓN, luego setear la aceleración, velocidad crucero, desaceleración y por último la posición final. Después de esta secuencia el motor acelerará hasta llegar a la velocidad crucero. Luego según la posición final a alcanzar, la velocidad y la desaceleración, calculará el tiempo durante el cual debe mantenerse en la velocidad crucero y cuando debe empezar a frenar ( desacelerar ) para llegar a la posición deseada.

Modo Velocidad

Este modo nos permite configurar el motor de tal manera que llegue a una velocidad y se mantenga en un movimiento constante en esta velocidad. Esta velocidad puede ser positiva o negativa y nos da los siguientes dos modos.

Avance

Para utilizarlo debemos configurar la llave de H en el modo VELOCIDAD+, luego setear la aceleración, el frenado y la velocidad crucero. Luego el motor acelerará hasta llegar a la velocidad especificada y se mantendrá en esta hasta recibir otra orden. Si la velocidad crucero seteada es menor que la actual, usará el valor del frenado para llegar a la misma.

Retroceso

El procedimiento es el mismo que en el de avance, pero debemos poner la llave H en el modo VELOCIDAD-.

Frenado

Este modo se usa cuando necesitamos frenar de forma abrupta el motor. La respuesta será un frenado del motor.

Controlador PID

Introducción

Un PID (Proporcional Integral Derivativo) es un mecanismo de control por realimentación. Un controlador PID corrige el error entre un valor medido y el valor que se quiere obtener calculándolo y luego sacando una acción correctora que puede ajustar al proceso acorde. El algoritmo de cálculo del control PID se da en tres parámetros distintos: el proporcional, el integral, y el derivativo. El valor Proporcional determina la reacción del error actual. El Integral genera una corrección proporcional a la integral del error, esto nos asegura que aplicando un esfuerzo de control suficiente, el error de seguimiento se reduce a cero. El Derivativo determina la reacción del tiempo en el que el error se produce. La suma de estas tres acciones es usada para ajustar la posición del motor, por medio de la modulación del ancho de pulso de sus llaves H. Ajustando estas tres constantes en el algoritmo de control del PID, el controlador puede proveer un control diseñado para lo que requiera el proceso a realizar. En el siguiente gráfico podemos identificar la posición, la cuenta y e(t) el error. Entonces el sistema actúa sobre el PWM, el cuál por medio de las llave H hace que el motor se mueva. El movimiento del motor modifica la cuenta y así el sistema se va corrigiendo hasta que el error se hace 0, esto quiere decir que la posición y la cuenta valen lo mismo.

Implementación

El funcionamiento interno del PID está descripto en la siguiente rutina.

   1 // ===============================================================
   2 // Filtro PID
   3 // ===============================================================
   4 void PID(void) {
   5    if(modo!='W') {
   6       GIE=0;
   7       U0=posicion-cuenta;
   8       GIE=1;
   9       // calculo de integral (solo si no ocurrio sobrecarga en el calculo de PWM anterior)
  10       if(!sobrecarga) {
  11          integral+=U0;
  12       }
  13       // calculo del PID
  14       Y=U0*KP+integral*KI+(U0-U1)*KV; 
  15       Y=Y*4;
  16       sobrecarga=0;
  17       if(Y>SATURAP) {
  18          Y=SATURAP;
  19          sobrecarga=1;
  20       } else if(Y<SATURAN) {
  21          Y=SATURAN;
  22          sobrecarga=1;
  23       }
  24       Y=Y>>8;
  25    } else {
  26       Y=valor_pwm;
  27       Y=Y<<8;
  28    }
  29 
  30    Y+=MEDIOPWM;
  31    if(Y>MAXIMOPWM){
  32         Y=MAXIMOPWM;
  33    } else if(Y<MINIMOPWM){
  34         Y=MINIMOPWM;
  35    }
  36    Y&=0xFFC0;
  37    pun1=(char *)&Y;
  38    // Si Y>512 adelante 
  39    // Si Y<512 atras 
  40    CCPR1L=(*(pun1+1));
  41    CCP1CON &=0b00001111;
  42    CCP1CON ||=(*(pun1+0)>>2);
  43    U1=U0;
  44    return;
  45 }

Como vemos al entrar al método PID lo primero que hace, desactiva las interrupciones, saca el error actual y lo guarda en U0=posicion-cuenta, luego procede a calcular el valor del PID:

   1 Y=U0*KP+integral*KI+(U0-U1)*KV;

• U0: Error actual

• KP: Constante Proporcional

• integral: la integral de los errores

• KI: Constante Integradora

• U1: Error anterior

• KV: Constante de Derivación

Luego se lo multiplica por 4, se comprueba que no pase ningún extremo¹.

Una vez chequeado, se hace la rotación de 8 bits y luego se le suma 32768, esto es una adaptacion de enteros para hacer mas simple la comparacion. Por último a este valor se lo escribe al registro pwm(verificando primero no exceder los límites del mismo² que es de 10 bits, por lo tanto si el valor es mayor a 512 el motor irá hacia una dirección y si es menor hacia la opuesta.

Ejemplos de Uso

Para testear el buen funcionamiento del carro se puede utilizar el programita agv.py, que es un pequeño script en python. El parámetro que se debe cambiar es la variable PORT, poniendo el puerto serie al que se encuentra conectado la llave H. Así si estamos en Windows se utliza com1 o 2, si estamos en GNU/Linux utilizando un adaptador de usb a serie, el parámetro por defecto debería funcionar. El script se lo invoca con el intérprete python, el nombre del programa y luego se le pasa como parámetro otro script en python que posee valores por defecto de las constantes para la sintonía del carro.

   1 python agv.py traccion.py

traccion.py tiene la siguiente forma

   1 modo = "POSICION"
   2 Ki = 1
   3 Kd = 0
   4 Kp = 30000
   5 ACELERACION = 15
   6 FRENADO =15
   7 VELOCIDAD_FINAL = 35000

Modo Posición

El siguiente fragmento de código es la secuencia que debemos seguir para poder usar la llave H en modo posición. Crea una instancia de AGV que la llama s, y luego le envía las secuencias prender y reset. Setea el modo posición y luego los registros Ki, Kd, Kp, ACELERACION, FRENADO, VELOCIDAD_FINAL, los trae del script que pasamos como parámetro que posee valores probados para una sintonía adecuada. Luego de tener los valores que nos definen la rampa, le seteamos la POSICION_FINAL y el carro comienza a moverse hasta que CUENTA coincide con el valor de POSICION_FINAL, siguiendo los valores de la rampa. Su velocidad crecerá con la aceleración seteada, luego tomará la velocidad crucero, e irá calculando en que momento debe empezar a desacelerar para poder llegar a la posición indicada, con el valor de frenado que le seteamos.

   1 if __name__ == 'main':
   2   PORT = "/dev/ttyUSB0"
   3   BAUDRATE = 9600
   4   try:
   5     modulo_config = sys.argv[1]
   6     if modulo_config.endswith(".py"): modulo_config = modulo_config[:len(modulo_config)-3:]
   7   except:
   8     print "hay que pasar un archivo de configuración"
   9   config = ''''import__(modulo_config)
  10   s = AGV(PORT, BAUDRATE)
  11   s.prender()
  12   s.reset()
  13   s.set_modo("POSICION")
  14   s.set_registro("Ki", config.Ki)
  15   s.set_registro("Kd", config.Kd)
  16   s.set_registro("Kp", config.Kp)
  17   s.set_registro("ACELERACION", config.ACELERACION)
  18   s.set_registro("FRENADO", config.FRENADO)
  19   s.set_registro("VELOCIDAD_FINAL", config.VELOCIDAD_FINAL)
  20   s.set_registro("POSICION_FINAL", 600)
  21   s.imprimir_registros("Ki", "Kd", "Kp", "ACELERACION", "POSICION_FINAL")
  22   for j in range(50):
  23     s.imprimir_registros("CUENTA", "VELOC_ACTUAL")
  24   s.set_registro("POSICION_FINAL", 0)
  25   for j in range(50):
  26     s.imprimir_registros("CUENTA", "VELOC_ACTUAL")
  27   s.reset()
  28   s.apagar()
  29 

Modo Velocidad

Ésta secuencia corresponde al modo Velocidad en avance, trae los valores que le pasamos desde el script, y le seteamos los valores de ACELERACION, FRENADO y VELOCIDAD_FINAL. El carro comenzará a incrementar su velocidad con la aceleración que le pasamos hasta llegar a la VELOCIDAD_FINAL, luego se mantiene en esta hasta que le pasemos otra VELOCIDAD_FINAL, que puede ser superior o inferior.

   1 if __name__ == '__main__':
   2   PORT = "/dev/ttyUSB0"
   3   BAUDRATE = 9600
   4   try:
   5     modulo_config = sys.argv[1]
   6     if modulo_config.endswith(".py"): modulo_config = modulo_config[:len(modulo_config)-3:]
   7   except:
   8     print "hay que pasar un archivo de configuración"
   9   config = __import__(modulo_config)
  10   s = AGV(PORT, BAUDRATE)
  11   s.prender()
  12   s.reset()
  13   s.set_modo("AVANZAR")
  14   s.set_registro("Ki", config.Ki)
  15   s.set_registro("Kd", config.Kd)
  16   s.set_registro("Kp", config.Kp)
  17   s.set_registro("ACELERACION", 15)
  18   s.set_registro("FRENADO", 15)
  19   s.set_registro("VELOCIDAD_FINAL", 10000)
  20   for j in range(50):
  21     s.imprimir_registros("CUENTA", "VELOC_ACTUAL")
  22   s.reset()
  23   s.apagar()

Para utilizar el modo velocidad en retroceso, se procede exáctamente igual, pero en el modo le ponemos el valor "RETROCEDER". Esto sería reemplazar la linea s.set_modo("AVANZAR") por s.set_modo("RETROCEDER")

Modo Frenado

Este modo se utiliza para frenar el carro, seteamos el modo en "FRENAR" y el carro se detiene (sobre esto tengo duda) con el valor de la desaceleración que tenga.