Robot Zero 2012. Encoders.

He terminado de montar y probar la electrónica del robot de este año y de momento parece que todo funciona, la principal diferencia respecto al robot del año pasado es la adición de los encoders, lo que nos va a permitir conocer no la velocidad, si no el tipo de trazado del circuito donde nos encontramos. Pongo un primer ejemplo de cómo usarlos.

Para el robot de este año hemos añadido los encoders que hicimos con los CNY70.

Son encoders con unas pocas líneas por vuelta, por lo que no nos van a servir para determinar la velocidad del robot velocista en la pista y cerrar así el lazo de control de velocidad de cada motor, para ello necesitaríamos encoders con muchos más pulsos como los que vienen integrados en los motores caros (el sueño de todo el que se monta robots en casa). Su importancia en este robot está en que con ellos vamos a poder determinar el trazado y contar el espacio recorrido para acelerar al principio de recta y frenar a final de ésta.

Para leer los encoders podemos usar los timers del microcontrolador o las interrupciones externas. Si usamos los timers por cada flanco de subida o de bajada en el pin del timer se incrementa un registro del microcontrolador donde lleva la cuenta de los pulsos. Los microcontroladores no suelen tener muchos timers (éste tiene 3 y usamos 2 para el pwm y el otro para el tic del programa) por lo que la mayoría de las veces vamos a tener que leerlos mediante interrupciones externas.

Para leerlos en este caso vamos a usar dos pines, que cuando cambia el valor en su entrada activa el flag de la interrupción habilitada en el pin llamando al vector de interrupción correspondiente.

Lo primero es configurar y habilitar las interrupciones del hardware de los pines del microcontrolador donde tenemos conectados los encoders, para ello vamos a usar los pines donde tenemos PCINT0 y PCINT23.

void inicializar_encoders(void)
{
   //Habilitar interrupciones de los encoers.
   PCICR |= (1<<PCIE0); // Interrupción ED
   PCMSK0 |= (1<<PCINT0); //Habilitar pin
   PCICR |= (1<<PCIE2); // Interrupción EI
   PCMSK2 |= (1<<PCINT23); //Habilitar pin
   sei(); //Habilitar interrupciones globales.
}

Las PCINT (pin change interrupt) son interrupciones asíncronas que suelen tener asociadas varios pines de entrada (fuentes de interrupción) al mismo vector de interrupción, por lo que una vez que se dispara la interrupción hay que determinar que pin del microcontrolador asociado a ese vector de interrupción la ha disparado. Este microcontrolador tiene 3 vectores de interrupción para 23 pines que pueden dispararlos, usando pines con distintos vectores de interrupción asociados nos ahorramos tener que determinar la fuente de disparo, y poder ejecutar la ISR en menos tiempo.

Cuando el valor del pin del encoder cambia se activa un bit (flag de interrupción) en un registro, si las interrupciones están habilitadas se salta a la dirección de memoria donde se ejecuta un código cada vez que se activa el bit anterior, y se limpia el registro anterior al terminar y volver al punto del programa desde donde se saltó. En este código de interrupción lo que vamos a hacer es incrementar una variable (o varias variables) que lleven la cuenta de los pulsos que han ocurrido en cada encoder.

volatile int encoderd; //Variables globales donde se cuentan
volatile int encoderi; //los pulsos de los encoders.
 
ISR(PCINT2_vect) //Izquierdo
{
     encoderi++; //Cuenta de pulsos.
//   PORTB ^= (1<<LEDV); //Conmutar led verde.
     PCIFR  |=(1<<PCIF2); //Limpiamos el flag de interrupción.
}
 
ISR(PCINT0_vect) //Derecho
{
     encoderd++; //Cuenta de pulsos.
//   PORTB ^= (1<<LEDR); //Conmutar led rojo.
     PCIFR  |=(1<<PCIF0); //Limpiamos el flag de interrupción.
}

Para llevar los pulsos declaramos unas variables de la longitud que estimemos oportuna. Si descomentamos las líneas de los leds podemos comprobar el funcionamiento de ambos encoders.

La ventaja de usar un timer es que no tenemos que estar llamando a una interrupción cada vez que ocurre un pulso en el encoder, el timer lleva la cuenta de manera automática en uno de sus registros sin que tenga que intervenir el microcontrolador. Cuando queramos ver cuantos pulsos han ocurrido en el encoder desde el microcontrolador sólo tenemos que leer el registro de la cuenta del timer. La desventaja es que los timers son unos recursos muy limitados en el microcontrolador, vamos a disponer de unos pocos, y los registros donde se llevan la cuenta son de un valor fijo, en los AVR puede ser un registro de 8 ó 16 bits.

La ventaja de usar las interrupciones externas para llevar la cuenta es que podemos usar los registros que queramos para llevar la cuenta del encoder, y estos pueden ser de la longitud que consideremos oportuna. Cuando ocurre un pulso incrementamos una o varias variables (según las necesidades de nuestro programa). Estas variables han de ser globales ya que las vamos a usar en distintas funciones, y volátiles, para no tener copias duplicadas en los registros internos del microcontrolador. La desventaja es que el microcontrolador tiene que estar atendiendo a los pulsos de los encoders.

En los AVR por defecto las interrupciones anidadas no están activadas, es decir una interrupción no va a poder interrumpir a otra, por lo que debemos asegurarnos que si estamos atendiendo a una interrupción del micro, no puedan saltar dos pulsos o más del mismo encoder, ya que estos pulsos se perderían y al salir de la interrupción en la que estabamos sólo veríamos el flag del encoder activado por el primer pulso que tuvo lugar. Si estamos por ejemplo en el código de interrupción del tic del programa y durante que éste se ejecuta, ocurren dos pulsos de los encoders (uno de cada uno), al salir de la interrupción del tic del programa el micro ve activados los flags de los dos encoders, por lo que entraría primero en la ISR del encoder de vector de interrupción de valor de memoria más bajo y al salir limpiar su flag, y posteriormente entraría en la interrupción del otro encoder haciendo lo mismo.

La velocidad de la rueda no es lo suficientemente alta para que puedan darse dos pulsos en el mismo encoder durante el tiempo que se ejecuta el código de una de las interrupciones del programa, por lo que no vamos a perder ningún pulsos de los encoders. A más líneas de los encoders mayor es la frecuencia de los pulsos, lo ideal será llevar toda las que podamos, cuantas más mejor, la limitación está en los disco que tenemos que hacerlos nosotros y en el ancho mínimo de línea que el CNY70 va a poder leer a una velocidad ángular dada. Para una primera prueba llevamos 8 líneas por encoder, lo que se traduce en 16 cambios de valor del pin de cada encoder por vuelta, un incremento en la variable de los encoders por cada 6 mm.

Una vez que tenemos los encoders funcionando se van a usar para dos cosas principalmente en este robot: para determinar el trazado, si estamos en recta o en curva, y el sentido de la curva y su curvatura. Y el otro uso que se le va a dar es para contar espacio y poder frenar antes de llegar al final de recta.

Una forma sencilla de hacer lo primero sería lo siguiente (lo primero que se me ocurrió probando la electrónica ayer..)

 //Determinar trazado.
if(encoderi > 25)
{
    trazado = encoderi - encoderd;
 
    if(trazado > 7) //Giro derecha
    {
        recta = 0;
    }
    else if(trazado < -7) //Giro izquierda
    {
        recta = 0;
    }
    else //Recta
    {
        recta = 1;
    }
 
    encoderd= 0;
    encoderi = 0;
}

En la interrupción principal del tic del programa, donde se ejecuta de forma periódica todas las funciones del robot, se mira si las variable del encoder de una de las ruedas ha llegado a 25 pulsos (15 cm), y si los ha recorrido se mira el valor de los pulsos acumulados en el encoder de la otra rueda. Comparando los valores de cada encoder podemos saber que rueda ha recorrido mayor distancia, lo que nos permite saber si el robot está en recta, en curva, qué sentido tiene esta curva, e incluso podríamos determinar el radio de curvatura para saber si estamos en una curva muy pequeña o grande. Todo esto es una forma sencilla de hacerlo, lo primero que salió ayer para probar la electrónica. Cuando nos pongamos con el programa habrá que ver cómo lo hacemos.

En el video se ve como funciona el código anterior. Cuando el robot está en recta apaga los dos leds, y enciende el led rojo cuando se encuentra en una curva a izquierdas y el led verde en una curva a derechas.

Una vez que sabemos si estamos en curva o en recta es donde podemos empezar a ajustar el comportamiento del robot. Podemos acelerar al máximo al principio de recta a la vez que llevamos la cuenta de los pulsos, para saber cuando se va a acabar la recta (suelen dar el circuito antes del concurso) y frenar antes de entrar a la curva. Podemos establecer una velocidad distinta para las curvas según su radio, así como poder establecer distintas constantes del comportamiento del robot según el tipo de trazado en donde se encuentre.

Por ejemplo una pequeña prueba con el código siguiente dentro de la interrupción del tic del programa de 1 ms y a continuación del código anterior.

//Establecer velocidad
 
if(recta != 0) //Estamos en recta
{
    if ((espacio > 100) && (espacio < 400))
    {
        velocidad = 173;
        //espacio ++;
        PORTB |= (1<<LEDR);
        PORTB &= ~(1<<LEDV);
    }
    else 
    {
        velocidad = 130;
        PORTB |= (1<<LEDV);
        PORTB &= ~(1<<LEDR);
    }
 
    espacio ++;
    if(espacio > 1000)
        espacio = 1000;
}
else //Estamos en curva
{
    velocidad = 155;
    espacio = 0;
    PORTB &= ~(1<<LEDR);
    PORTB &= ~(1<<LEDV);
}

Por ejemplo en el código anterior cuando el robot está en curva cambia su velocidad respecto a recta y apaga los dos leds. Cuando está en recta acelera durante 300 ms a una velocidad mayor ecendiendo el led rojo y transcurrido este tiempo baja a la velocidad de recta para no salirse en la entrada a la curva y enciende el led verde.

Sólo es una pequeña prueba, todo ésto hay que porgramarlo despacito y bien, ya que en una carrera de 3 minutos se pueden dar 40 ó 50 vueltas, por lo que una diferencia de unos pocos milisegundos entre dos robots supone ganar o perder la carrera.

Con la adición de los encoders el robot se vuelve mucho más entretenido, y el límite para bajar décimas por vuelta lo pone el tiempo que echemos y nuestra imaginación.

La primera impresión del robot de este año es buena, aún no lo hemos puesto lastre para ver cuanto podemos correr, pero yo diría que va a ir más rápido que el año pasado. Cuando probemos un poco más la placa y tenga ganas.. haré y subiré las versión final de la placa con los archivos de Itead para pedir y la lista de componentes, por si a alguien le interesa que se lo pueda hacer.

Feliz 2012.

Visitas :9250
Both comments and pings are currently closed.

5 Responses to “Robot Zero 2012. Encoders.”

  1. Oscar says:

    Este avance puede suponer recorrer los 3m/s o más

  2. Jorge says:

    No sé que velocidad podrá hacer dependiendo de la pista, ya que la rueda es muy mejorable. Pero lo que es seguro que tenemos robot para entretenernos todo el 2012.

  3. [...] how much a wheel has turned in your project? Then you need a rotary encoder! Here’s a way to add rotary encoding without changing the mounting method of your wheels (translated). [Jorge] added it as a way to improve the functionality of this line-following robot. [...]

  4. Gremio says:

    Que buena pinta, fantástico trabajo, enhorabuena por el tutorial y el resultado.

    Ahora ando liado y no estoy en mi casa, de aquí a un par de semanas ya estaré de nuevo operativo 100% y no faltare de poner una entrada en mi blog sobre C.I.r.E,

    • JMN says:

      Gracias,

      la verdad que este año parece que el robot puede correr, con un programita simple de dos velocidades constates, y sin centrar las masas/inercias con lastre ni usar los frenos (que es lo que va a dar resultado) ya está a 2.4 m/s de velocidad media.

      http://www.youtube.com/watch?v=Ila0g68w_eI

      La pena es que han salido los premios del Cosmobot hoy y los han dividido entre cinco! por lo que seguro que el concurso y hacer velocistas interesa menos, aún así desarrollaremos el robot a lo largo del año.

      Saludos.

Subscribe to RSS Feed Follow me on Twitter!