MiniZ: medir la velocidad.



Uno de los principales problemas del miniZ en concursos pasados es que no era capaz de conocer la velocidad a la que iba, lo que no permitia acelerar hasta alcanzar la velocidad deseada para las distintas situaciones, y también la velocidad dependía del estado de la batería. La forma más fácil de solucionar los problemas anteriores es crear un par de encoders ópticos que lean unos discos situados en la ruedas traseras, lo que nos va a permitir conocer la velocidad a la que se desplaza el velocista. Los discos están formados por rayas negras y blancas, de tal forma que la salida del sensor estará a 1 ó 0 según el color de la parte del disco que se encuentre delante (blanco o negro), lo que nos proporcionará un tren de pulsos cuya frecuencia es función de la velocidad de la rueda.

Como sensor se usa un CNY70, el esquema del montaje es muy sencillo:



Dos conectores para los CNY70, las resistencias necesarias para su funcionamiento, dos leds con sus resitencias a la salida del circuito para comprobar de forma visual que los encoder están funcionado y un 74HCT14 para tener una odan cuadrada a partir de la señal generada por los sensores y también para poder alimentar los leds. Alimentando a una tensión baja podemos reducir las tensiones de umbral del 74HCT14, lo que nos permitiría detectar líneas más finas en el disco y tener una mayor resolución, el disco tiene seis líneas negras y seis blancas lo que es más que suficiente para tener una medida precisa de la velocidad en uno de estos coches funcionando como velocista.

PCB.

Fotos del montaje:




Los CNY van pegados sobre las manguetas traseras muy cerca de la rueda, sobre la que se ha hecho un disco en su interior.

Video del funcionamiento:
1. Prueba de los encoders.
2. Salida del CNY70.
3. Salida del circuito.
4. Salida del circuito menor velocidad.













Programa para la prueba del enconder.

Para usar el encoder debemos de ser capaces de contar el número de pulsos que tienen lugar en un periodo de tiempo, para ello se usan dos de los timers del microcontrolador, explicados en servomotor, uno se encarga de generar una interrupción cada 16 ms y otro de llevar la cuenta de los pulsos que han tenido lugar en este periodo de tiempo.

Los 16 ms se eligen debido a que es el periodo de la señal de control del miniz y por tanto aprovechamos esta interrupción de 16 ms para contar los pulsos que han tenido lugar en el encoder.

Lo primero es configurar los timers, para la interrupción se usa un tiemer de 16 bits, el cuatro (con un cristal de 16MHz y uno timer de 8 el tiempo máximo a  contar es de algo más de 16 ms por lo que también valdría) y para llevar la cuenta de los pulsos un timer de 8 bits, el cero.

void inicializar_timer4(void) //Configura el timer y la interrupción.
{
    OCR4A= 0x0F9F;
    TCCR4B |=((1<<WGM42)|(1<<CS41)|(1<<CS40));    //Los bits que no se tocan a 0 por defecto
    TIMSK4 |= (1<<OCIE4A);
    sei();
}

En la configuración del timer de 16 bits se selecciona el reloj interno como entrada al timer con su prescaler correspondiente, modo de funcionamiento, y se habilita la interrupción para lanzar cada 16 ms la rutina de interrupción contando los pulsos entre las distintas tareas necesarias para el control del coche.

void inicializar_timer0(void)
{
    TCCR0B |=((1<<CS02)|(1<<CS01)|(1<<CS00));     //Normal Mode y reloj externo flanco de subida
}

En la configuración del timer0 se selecciona el modo de funcionamiento y un reloj externo que se conecta al pin correspondiente a la entrada de reloj externo para el timer0 del microcontrolador. Este reloj externo es la señal generada por el encoder e incrementará la cuenta del registro del timer en cada flanco de subida o en cada flanco de bajada según lo seleccionemos.

ISR(TIMER4_COMPA_vect)
{
    unsigned int cuenta_actual=TCNT0;
    if (cuenta_actual>cuenta_old)
    {
        cuenta=cuenta_actual-cuenta_old;
        suma_cuenta=suma_cuenta+cuenta;
    }
    else if(cuenta_actual<cuenta_old)
    {
        cuenta=256+cuenta_actual-cuenta_old;
        suma_cuenta=suma_cuenta+cuenta;
    }
    cuenta_old=TCNT0;
    contador++;
    TIFR4 |= (1<<OCF4A);
}

Cada 16 ms se lanza esta ISR donde contamos el incremento de pulsos que ha tenido lugar en el registro TCNT0 (registro del timer0 que lleva la cuenta del encoder) y almacenamos esta cuenta en una variable (cuenta, suma_cuenta, con una de las dos valdría) para su posterior utilización en el búcle principal del main(). En lugar de resetear el registro TCNT0 es mejor calcular el incremento de pulsos, de esta forma no perdemos la cuenta de ningún pulso como podría ocurrir si el paso de 1 a 0 del encoder sucede en el mismo instánte que reseteamos el registro TCNT0. La situación en la que cuenta_actual = a cuenta_old no es posible físicamente debido a la velocidad de la rueda, en 16 ms no podemos tener 256 pulsos.

    while(1)
    {
    if(contador>62)
    {
        suma_cuenta_local=suma_cuenta;
        contador=0;
        suma_cuenta=0;

        posicionar_cursor(2,13);
        escribir_valor_decimal(suma_cuenta_local);

        velocidad= suma_cuenta_local*12;
        posicionar_cursor(3,12);
        escribir_valor_decimal(velocidad);

        espacio= espacio + (unsigned long int)suma_cuenta_local*12036;
        posicionar_cursor(4,10);
        escribir_espacio(espacio);
    }   
    }

En el búcle principal se imprime por la pantalla del LCD la cuenta de los pulsos, el valor de la velocidad de la rueda y el espacio que recorrido por el coche para comprobar el correcto funcionamiento del encoder. Se hace cada 63 llamadas (63*15.9994ms) a la interrupción del timer4 para representar las medidas tomadas por el enconder en la pantalla del LCD cada 1 segundo.

La cuenta de los pulsos se corresponde con el número de pulsos producidos por el encoder que han tenido lugar durante el último segundo, lo que coincidirá con el valor de la frecuencia de la señal del encoder observada en un osciloscopio, de esta forma comprobamos el correcto funcionamiento del programa.

Como radio de la rueda se han tomado 1.15 cm, el disco del encoder está formado por seis líneas blancas y seis lineas negras, es decir en una revolución de la rueda tendremos 6 pulsos. Lo que hace que en un pulso la rueda recorra un valor de 2*pi*1.15/6 = 1.20366 cm sobre el suelo, con esta medida del espacio que se recorre por cada pulso se determina la velocidad de la rueda sobre el suelo y el espacio recorrido, para el espacio cuanto menor sea el error cometido en las aproximaciones mejor ya que es acumulativo, en este caso como ejemplo para su cálculo se hace en pasos de 12036 um. La variable máxima para acumular el espacio recorrido sería un unsigned long long int que nos permitiría acumular 2^64 valores.

código completo.

Ejemplo de funcionamiento.


Para cualquier comentario, duda, correción:

blog comments powered by Disqus