Medicin de velocidad: encders

Diseamos el mdulo que permitir conocer de forma exacta (salvo

errores de medida) la velocidad del robot: el encder ptico. Gracias a
este dispositivo, podremos medir la velocidad angular de la rueda, y a
partir de ah calcular la velocidad lineal del robot, conocido el radio de la
rueda.
Funcionamiento
Las ruedas del robot disponen de una serie de
dientes (12 en concreto) situados en la zona
interna de la rueda, y distribuidos de manera
uniforme a lo largo del permetro de la misma.
Podemos verlos en la siguiente imagen:
Para medir la velocidad angular, basta con medir
el tiempo transcurrido entre el paso de un
encoder y el siguiente por una referencia.
Multiplicando por el nmero de dientes esta medida, obtenemos el
periodo de la rueda.
Diseo del encder ptico
Para poder detectar el paso de cada uno de los dientes, diseamos un
encder ptico. Dispone principalmente de un detector fotorreflectivo,
que emite luz infrarroja y genera una respuesta en tensin proporcional
a la luz recibida. En caso de que la luz rebote sobre una superficie
altamente reflectante (por ejemplo, los dientes que vemos en la rueda)
la salida tomar un nivel alto, y a la inversa si se trata de una superficie
oscura (los huecos entre los dientes).
El diseo del encder es muy sencillo, tal y como vemos en el siguiente
esquema:

El elemento ptico es un QRE1113, similar al conocido CNY70, pero de

dimensiones mucho ms reducidas. Se compone sencillamente de un
LED infrarrojo y un fototransistor. La salida se conecta a un buffer con
histresis (74LVC1G17, de Texas Instruments) para disponer de una
salida final con flancos definidos y sin glitches. La alimentacin del
mdulo ser a 3.3V, dado que todo el sistema trabaja con estas
tensiones.
El diseo de la PCB se ha realizado en Eagle, y la fabricacin ha sido
casera, cuyo resultado vemos en la figura de la derecha.
Resultados
Acoplamos el encder ptico a la rueda tal y como vemos en la imagen:

Finalmente, medimos la salida (cable azul) en el osciloscopio del

laboratorio, con el motor girando al 30% de su capacidad (1300 rpm*0,3
= 390 rpm):

Vemos una salida pulsante, como se predeca. No es perfectamente

cuadrada por la situacin del QRE con respecto a la rueda, pero lo que
nos importa es la frecuencia de la seal. Vemos que en la medida
tenemos una frecuencia de 77,22 Hz, luego la frecuencia de la rueda
ser de 77,22/12 = 6,435 revoluciones por segundo. Finalmente,
medimos que la rueda gira a 6,435*60 = 386,1 rpm ~ 390 rpm(terico).
La diferencia se deber fundamentalmente a cuestiones mecnicas.
Implementamos ahora en el microcontrolador las rutinas para calcular el
tiempo entre flancos de subida.
La primera idea que tuvimos fue hacerlo por captura de entrada,
empleando un Timer para cada uno de los dos encders. El problema es
la ocupacin actual de los Timers del microcontrolador:
-Los dos timers de 16 bits se emplean para el PWM
-Uno de los dos timers de 32 bits se emplea para generar retardos.
-El contador de 24 bits Systick se utiliza para funciones de control del
PID.
Por lo tanto, slo nos sobra uno de los Timers de 32 bits, con el cual
determinaremos el tiempo de ambos motores. El algoritmo ser el
siguiente:

-Configuramos el Timer para generar una interrupcin en tiempo

real.
-Cada vez que se ejecuta la rutina de interrupcin, aumentamos en una
unidad dos contadores, uno para cada encoder. Con esto podemos
contar tiempo en funcin del nmero de ticks de la interrupcin.
-Por otra parte, conectamos las salidas de ambos encders a puertos
GPIO con configuracin deinterrupcin externa, por flanco de subida o
de bajada. Conectaremos la salida de los encders a los pines PIO2_1 y
PIO2_4, generando una interrupcin por flanco.
-Cada vez que se produzca una interrupcin GPIO, lo cual significa que
hay un nuevo flanco de subida/bajada, capturamos el valor de cuenta
del Timer, lo sumamos a una variable temporal, y reiniciamos la cuenta
del Timer. Cuando se han producido 4 interrupciones, dividimos por 4 la
variable temporal, y el resultado lo utilizaremos para calcular el tiempo.
Es decir, hacemos una media de 4 medidas de tiempo.
-Finalmente, una vez obtenida la cuenta del Timer en la variable
temporal, transformamos el nmero de ticks a segundos, con los que
calculamos directamente la velocidad de la rueda. La velocidad global
del robot ser la media de las velocidades de ambos motores.
Lo vemos en un diagrama de flujo:

Las funciones que implementamos son las siguientes:

timer32_1_init()

Esta rutina configura el segundo Timer de 32 bits para generar la

interrupcin peridica. Lo configuramos para que interrumpa cada 100
us, teniendo en cuenta que el sistema funciona a 72 MHz, con
PRESCALER = 71, TMR32B1_MR0 = 100. El error mximo cometido
tendr lugar al medir el tiempo entre dientes con la rueda girando a su
mxima velocidad, 1300 rpm tericos. Esto se traduce en un tiempo
entre dientes de unos 3.84 ms, luego el error mximo cometido es de un
2,6 % aproximadamente, luego el resultado obtenido ser muy bueno.
TIMER32_1_IRQHandler()

Simplemente
actualizamos
las
variables
cuentaEncoderR
cuentaEncoderL, sin olvidarnos de resetear el flag de interrupcin.

Disponemos de funciones auxiliares para setear y obtener los valores de

las cuentas.
PIOINT2_IRQHandler(void)

Realiza las operaciones mostradas en el diagrama de flujo.

uint16_t calculateSpeed(uint32_t timeCount)

Calcula la velocidad dado el nmero de ticks. Primero realiza la

conversin de ticks a tiempo (en microsegundos), y a continuacin
obtiene
la
velocidad
en
funcin
del
nmero
de
dientes
NUM_DIVISIONS_WHEEL y el radio de la rueda WHEEL_RADIO_MM. La
velocidad se expresa en mm/s para tener mayor precisin, ya que
trabajamos con enteros. Posteriormente, el Tablet lo reconvertir a m/s.
Finalmente, obtendremos la velocidad del robot haciendo la media de las
velocidades del motor izquierdo y derecho, y es lo que enviaremos por
Bluetooth al sistema.