Codificadores en cuadratura

Tras un largo paréntesis impuesto por el trabajo, retomo el blog. En las siguientes entradas vamos a aplicar la salida PWM del micro a un motor DC con capacidad para manejarlo en ambas direcciones.  Nuestro objetivo final será escribir un código de control para hacer que el motor alcance una posición dada y se detenga o se mueva con velocidad constante.  Para ello necesitaremos algún tipo de información sobre la posición del motor y su velocidad. La forma más común de obtener dicha información es a través de codificadores en cuadratura. 

En esta entrada describiremos su concepto y los diferentes modos en los que podemos monitorizarlos (1X, 2X, 4X).

Archivos de código asociado a esta entrada:   quad1.c

Una típica forma de obtener información de un motor (u otro dispositivo giratorio) en cuanto a su posición, velocidad y dirección es el uso de 2 codificadores en cuadratura (quad encoders), normalmente etiquetados como canal A y B.

Un motor con encoders contará típicamente con 6 conexiones:

·          M+ y M-  : las de un motor normal (conectadas a las salidas OUT1 y OUT2 del L293)

·          Alimentación de los encoders (a niveles lógicos):  Vc (5 o 3.3V) y GND.

·          2 salidas con los datos de ambos canales, CH_A y CH_B que conectaremos a sendos pines (configurados como entradas) del microcontrolador.

Las fotos siguientes muestran dos motores que he usado en los siguientes ejemplos.  El de la izquierda (Maxon GM20) es un pequeño motor muy bien acabado y relativamente caro. El de la foto de la derecha es un motor más grande y económico (unos $35, http://www.pololu.com/catalog/category/116).  Sin embargo, de cara a estos ejemplos, ambos cuentan con codificadores de dos canales en cuadratura y 6 conexiones como las descritas antes, por lo que son electricamente intercambiables (ambos funcionan nominalmente a 6V)

La figura siguiente ilustra el funcionamiento de un encoger con 2 canales, A y B. Los canales pueden verse como 2 sensores (p.e. de luz) fijos. Una pantalla opaca gira con el eje del motor y alternativamente "eclipsa" a uno u otro sensor. Podemos suponer que por defecto los canales están a un nivel alto y cuando son "tapados" caen a un nivel bajo. En la ilustración siguiente los sensores están posicionados a 90º y la pantalla cubre media circunferencia.

La figura ilustra la salida de los sensores cuando el eje gira en sentido antihorario. Las fechas marcan los eventos (caídas o ascensos de un canal) al ir tapándose o liberándose los sensores. El resultado es un par de señales (ch A y ch B) con una serie de pulsos en cuadratura (desplazamiento de fase de 90º o un cuarto de velta).

La frecuencia de los pulsos nos indica la velocidad de giro del motor. En este caso simplificado cada periodo de ch A o ch B corresponde exactamente a una vuelta del eje.

¿Para que son necesarios dos canales? Con uno solo podemos medir velocidad de giro pero no su sentido. Al tener dos canales en cuadratura, dependiendo del sentido de giro, uno se eclipsará antes que el otro. En este caso de giro antihorario lo que le pase a A le pasará un poco más tarde (un cuarto de vuelta después) a B. La señal en A precede a la del canal B.

¿Como programaríamos nuestro microcontrolador para detectar la posición del motor? Lo más eficaz sería conectar el canal A a RB0 y usar una interrupción que se dispara ante un cambio externo de un pin (p.e. INT0 asociada a cambios de RB0).  Activaríamos la interrupción INT0 y la configuraríamos para p.e. saltar ante una subida. Cada vez que A pase de 0 a 1 se dispararía la interrupción y se incrementará/decrementará un contador de vueltas.

¿Cómo podemos saber si incrementar o decrementar el contador? En la siguiente figura ilustramos el caso contrario, cuando giramos en el sentido horario y el canal B es el que va adelantado.

Supongamos que monitorizamos el canal A (el conectado a RB0), no importa si la subida o la bajada. Si nos fijamos en la figura inicial vemos que cada vez que hay un cambio en A, justo a continuación, los canales se encuentran en estados opuestos. Por el contrario, en la segunda figura, tras cada cambio de A (1 o 3) ambos canales están en el mismo estado. Es esa condición la que usaremos para detectar el sentido del giro y decidir si incrementar o disminuir  el contador

El siguiente código ilustra como codificaríamos esto dentro de la rutina asociada a la interrupción del INT0: 

void high_ISR (void)

{

 if (INT0_flag)   // INT0 ISR (keeps track of position)

    {

     if (PORTBbits.RB1==PORTBbits.RB0) quad++; else quad--;  // 1x encoder

     INT0_flag=0;

    }

}

El código incrementa una variable (quad) si RB0 (ch A) == RB1 (ch B), esto es, si estamos girando en sentido horario. En caso contrario se decrementa el contador. 

Encoder en modo 2X:

El código anterior corresponde a lo que se denomina un encoder 1X, ya que solo monitorizamos bien las subidas o las bajadas del canal A, pero no ambas. En el caso descrito, la resolución es de 1 vuelta del eje, ya que el canal A se eclipsa 1 vez por vuelta.

Si deseamos más resolución podemos monitorizar tanto las bajadas como las subidas. En ambos casos haremos lo mismo que antes, comparar el estado de RB0 y RB1 y aumentar/disminuir el contador. En este caso como A sube y baja 2 veces por vuelta tendremos el doble (2X) de resolución.

El PIC solo permite configurar la interrupción como de subida o de bajada. ¿Cómo podemos detectar tanto las subidas como las bajadas? Sencillo: basta cambiar la configuración de la interrupción cada vez que suceda (esto es, dentro del código de la ISR).

El código quedaría:

void high_ISR (void)

{

 if (INT0_flag)   // INT0 ISR (keeps track of position)

    {

     if (PORTBbits.RB1==PORTBbits.RB0) quad++; else quad--; 

     INTCON2bits.INTEDG0=~INTCON2bits.INTEDG0; // 2X encoder

     INT0_flag=0;

    }

}

Basta añadir una línea que cambia el bit que define el sentido de la detección (INTCON2.INTEDG0). Si entramos por una subida, se cambia y la próxima vez detectaremos la bajada. Cuando volvamos a entrar por una bajada, se quedará configurado para saltar a la siguiente subida. En resumen, atrapamos todos los cambios (y sin tener que preocuparnos por nada ya que todo lo hace la interrupción).  La línea de incrementar/decrementar el contador no hace falta cambiarla ya que la condición de chA=chB o chA <>chB no cambiaba en subidas o bajadas.

Usar un encoder 2X es muy recomendable ya que:

·   No requiere hardware ni interrupciones adicionales (usamos la misma interrupción INT0 que antes).

·   Sólo requiere una línea adicional de código.

·   Duplica la resolución de nuestro contador (ahora cuenta en 1/2 vueltas).

Además de la mayor resolución hay una razón más importante para usar siempre por defecto un encoger 2X.

Un encoder 1X es perfectamente válido (salvo por su menor resolución) en el caso de un motor que se mueve en una sola dirección. Sin embargo puede introducir errores si nuestro motor puede cambiar su dirección. Consideremos la figura siguiente:

Tenemos un movimiento de vaiven y el borde de la pantalla tapa y libera alternativamente al sensor A (el sensor B está siempre libre). Esto provoca una serie de pulsos en A y ningún cambio en B.

Con un encoder 1X vigilando p.e. las subidas de A contaríamos 2 subidas de A (verificando que chA=1=chB) que causarían 2 incrementos del contador, cuando claramente no hemos completado ninguna vuelta.

En cambio, con un encoder 2X también veríamos 2 bajadas de A (verificando esta vez que chA=0<>1=chB) por lo que decrementaríamos el contador en dos ocasiones, cancelando los incrementos anteriores.

Esta (y no la mejora de la resolución que puede no ser importante en nuestra aplicación) es la razón por la que se recomienda el uso de un codificador 2X en estas aplicaciones.

Encoder en modo 4X:

Si por alguna razón deseamos mayor resolución en nuestro contador podemos implementar un encoder 4X. Se trata de monitorizar eventos (subidas y bajadas) en ambos canales simultáneamente.

Precisamos eso si, que el canal B esté conectado a otro pin con capacidad de interrupción (por ejemplo RB1, usando la interrupción INT1). Ahora tenemos que escribir código para las dos posibles interrupciones:

if (INT0_flag) // RB0 (ch A) change

 {

  if (PORTBbits.RB1==PORTBbits.RB0) quad++; else quad--; 

  INTCON2bits.INTEDG0=~ INTCON2bits.INTEDG0; // 2X encoder

  INT0_flag=0;

 }

if (INT1_flag) // RB1 (ch B) change

 {

  if (PORTBbits.RB1==PORTBbits.RB0) quad--; else quad++;   

  INTCON2bits.INTEDG0=~ INTCON2bits.INTEDG0; // 2X encoder

  INT1_flag=0;

 }

Al contrario que el encoder 2X, el uso de un encoder 4X si que tiene un "gasto" adicional. Necesitamos que la segunda línea (ch B) también tenga asociada una interrupción. Esto no era necesario en el caso 2X donde la segunda línea (ch B) podría ser cualquier pin definido como entrada.

Por ejemplo, en el caso del PIC 2520/4520 tenemos 3 interrupciones externas INT0 (RB0), INT1 (RB1) e INT2 (RB2) por lo que podríamos monitorizar hasta 3 pares de canales (3 motores) en modo 2X (conectando los respectivos canales A a los tres pines citados). Por el contrario, en modo 4X sólo podríamos monitorizar un solo motor (ya que gastaríamos RB0 y RB1 para ese fin.

En los programas siguientes de control de motores usaremos por defecto los encoders configurados en modo 2X.

Encoders con más de un ciclo por vuelta

Por simplicidad hemos ilustrados los ejemplos anteriores con el caso de un par de sensores separados por 90º y una pantalla de 180º lo que provoca una resolución de 1 vuelta (modo 1X), 1/2 vuelta (2X) o 1/4 vuelta (4X).

Es posible aumentar la resolución "por hardware", situando más cerca los sensores A y B y usando una pantalla donde se alternen varios periodos de paso/bloqueo en cada vuelta. Algo así como lo ilustrado en la figura:

Esto corresponde a una configuración que en modo 1X tendrá una resolución de 10 unidades (de 36º) por vuelta (las 10 aspas del gráfico) y en modo 2X de 20 unidades (18º) por vuelta.

Las aspas pueden ser efectivamente una "hélice" que eclipsa/deja pasar alternativamente la luz sobre una pareja de fotodiodos (sensores A y B). Esto sucedía así en el caso de los viejos ratones de bola. Más comúnmente, los sensores A y B son un par de sensores Hall de campo magnético. Las "aspas" son entonces una serie de imanes de polos alternados N y S que hacen cambiar el estado de los sensores de forma alternativa.

Además, en muchos motores los sensores de cuadratura están montados sobre el eje "directo" del motor. Si luego el motor tiene una reductora, la resolución sobre el eje de salida de la reductora se incrementa notablemente. Por ejemplo, el motor GM-20 de Maxon que uso en estos ejemplos tiene un encoder magnético de 16 pulsos por vuelta y una reductora de 55.1:1. Eso se traduce en que cada vuelta del eje de salida son 55.1 vueltas del eje motor y por consiguiente 55.1x16 = 881.6 pulsos del encoder (en modo 1X). En modo 2X tendríamos unos 1765 ticks por vuelta, lo que corresponde a una resolución de 360º/1765 = 0.2º.

Implementación: quad1.c

El programa quad1 implementa una lectura de los canales A y B de un motor. En esta primera prueba solo conectaremos las salidas de los canales A y B (a RB0 y RB1 respectivamente) y la alimentación (5V) y masa

de la lógica de los sensores. No conectaremos las conexiones del motor y lo moveremos a mano para verificar la lectura de los canales

Como nuestro objetivo final en próximas entradas será un sistema de control, quad1.c está escrito con esa idea en mente. Además de las interrupciones para contar los pulsos de los canales, tendremos otra interrupción asociada a un timer (TMR0). A dicha rutina se le suele denominar "código del servo", ya que en una aplicación de control, será la encargada de monitorizar lo que está haciendo el motor y actuar en consecuencia. En este primer ejemplo lo único que hará será determinar la velocidad y posición del motor (contador del encoder).

En principio, la velocidad del motor se expresará en cuantas "ticks" se hayan contado durante el ciclo del servo. Para pasar de dicha velocidad a otras medidas como revoluciones por minuto (rpm) o similares tendremos que conocer los datos particulares de nuestro motor (ticks/vuelta), si estamos en modo 1X,2X o 4X y finalmente la frecuencia con que se ejecuta el código del servo.

La interrupción del canal A (INT0) se declarará de ALTA prioridad, mientras que la del servo (TMR0) será de BAJA prioridad. La razón es que no podemos permitirnos perder ningún pulso por lo que la llegada de un pulso debe interrumpir cualquier otra tarea.

En relación con esto también es necesario considerar de que tipo vamos a definir la variable quad que llevará la cuenta de los pulsos (o medios pulsos en modo 2X) recibidos. En principio, en un motor girando dicho contador puede ser muy alto, por lo que podríamos usar un entero con signo de 32 bits (int32). Sin embargo, eso hace que en cada interrupción INT0 (en la que nos interesa demorarnos lo menos posible) tengamos que incrementar o decrementar un entero de 4 bytes. 

Otro problema sería que mientras estamos accediendo a ese contador desde la rutina del servo, podríamos ser interrumpidos por INT0 y dicho contador modificado. Esto podría llegar a corromper nuestros datos.

Para evitar estos problemas una alternativa es declarar quad de tipo más pequeño, int16 o incluso como un byte. Las ventajas son:

·    La interrupción INT0 dura lo menos posible ya que solo tiene que incrementar un byte.

·    Cuando accedamos al valor de quad en la rutina del servo, al ser un único byte lo leeremos en una sola instrucción, por lo que no hay problemas de quedarnos a medias del proceso.

La desventaja es que si el motor gira muy rápido, es posible que un byte sea insuficiente. Será suficiente si el incremento/decremento del contador en un ciclo del servo es inferior a 128. En la rutina del servo compararemos 

el valor del contador con el último y determinaremos cuanto se ha movido y hacia donde.

El siguiente código corresponde a la rutina del servo, que en este ejemplo (reloj de 20 Mhz, preescaler 1:2 en TMR0 y un contador de 50000 entre cada cada interrupción) se ejecuta con una frecuencia de 50 Hz:

         Fservo = (Fosc/4)/(Pre * cont) = 5Mhz / 100000 = 50 Hz

void low_ISR (void)

{

 uint8 temp;

 if (TMR0_flag)   // ISR TMR0 (servo)

  {

   set_TMR0(15536);  // T = 50000 ticks with 1:2 PS @ 20MHz = 50 Hz

   PORTCbits.RC0=~PORTCbits.RC0;

    

   temp=quad; delta=temp-last_quad;  // delta = delta counter

   last_quad=temp;

   if (delta&gt;128) delta-=256;        // correct for rollover

   else if (delta<-128 data-blogger-escaped-delta="" data-blogger-escaped-nbsp="" data-blogger-escaped-span="">

   pos=pos+delta;                    // increment position

   TMR0_flag=0;

  }  

}

El código calcula la diferencia entre sucesivas observaciones (delta) y lo acumula en una variable de posición (está si un entero de 4 bytes, int32). El código detecta posibles rollovers de la variable quad. Si la diferencia (delta) entre observaciones excede +128 o -128 asumimos que el contador ha dado la vuelta y corregimos en consecuencia (restando o sumando 256). Es por esto por lo que no es posible contabilizar velocidades mayores de +/-128 en cada ciclo del servo.  

Si nuestro motor puede girar muy rápido (o tiene muchos pulsos por vuelta) deberemos reducir el ciclo del servo para evitar overflow. Si esto no es posible no nos quedará más remedio que declarar quad de mayor tamaño. Un entero de 16 bits será suficiente en la mayoría de los casos.

El programa principal simplemente arranca el timer TMR0 y configura adecuadamente las interrupciones. La comunicación es a través del puerto serie a 9600 bauds. En el bucle principal el programa vuelca por el puerto serie los valores de la posición y velocidad (delta) del motor, ambos en pulsos del encoder.  Se ha añadido la opción de elegir entre modo 1X y 2X pulsando 1 ó 2 en el terminal.

El motor usado en este ejemplo es un MAXON GM-20 con un encoder de 16 pulsos por vuelta y una reductora de 55.1:1. Por lo tanto, en modo 1X tendremos 16 x 55.1 = 881.6 ticks por vuelta. En modo 2X subiremos al doble, 1763 ticks/vuelta.  La frecuencia del servo es de 50 Hz (20 ms) por lo que para pasar del valor delta obtenido a

revoluciones por minuto haríamos:

      vueltas  = delta / 1763     tiempo  =  20 ms                  

                                             
      rps = 50 x delta / 1763   

                           

      rpm = 60x (50 x delta) / 1763  = 1.7015 x delta

Este motor en sus especificaciones recomienda un máximo de 200 rpm en su eje de salida (tras la reductora) para 6V. Eso corresponde a unas 3.algo rps  = 6000 ticks/sec . Con una frecuencia del servo de 50 Hz el máximo incremento sería de 6000/50 = 120. Por lo tanto, estaríamos dentro de las limitaciones de nuestro enfoque de usar una variable de tipo byte como contador.

El video final ilustra el funcionamiento del programa, moviendo el motor de forma manual. Se muestra como en modo 2X una vuelta completa corresponde a 1765 "ticks" y a la mitad (unos 880) en modo 1X. Conectando los bornes del motor a una batería observamos una velocidad de 90 ticks (modo 2X) por periodo del servo, lo que se traduce  a unas 1.70 x 90 = 150 rpm.