Derivada del error y control proporcional derivativo. En
la actualización anterior se leían los sensores para
localizar la posición de la línea, y dependiendo de lo
lejos que la línea estuviera del centro de la placa de sensores
se asignaba un error. Ese error es proporcional a la distancia de la
línea al centro y lo usaremos para establecer un control
proporcional, cuanto más lejos más gira el robot para
centrarse en la línea. El problema con este controlador
proporcional es que nunca conseguiremos que el robot se centre sobre la
línea, siempre va a estar oscilando sobre ésta como se
podía apreciar en el video, el robot llega, se pasa y corrige, y
así indefinidamente.
Para conseguir que el robot se centre sobre la línea y no oscile
tenemos que implementar un control derivativo (además del
proporcional), para ello se ha de conocer la derivada del error, es
decir como cambia el error respecto al tiempo, que es lo mismo que la
velocidad con que la línea se desplaza entre los sensores. Con
un control derivativo nos podemos anticipar al error futuro, observando
como varía éste y anticipando la respuesta antes de que
se produzca obteniendo su derivada.
Lo primero para poder medir la velocidad con la que cambia el error es
tener la capacidad de medir el tiempo que ha pasado entre dos medidas
de error ((velocidad de cambio = (error_anterior - error)/tiempo), para
ello tenemos que usar un timer. Se va a crear una interrupción
principal para que el programa que controla el robot se ejecute en
intervalos de tiempo fijos, de esta forma entre una medida y otra de la
placa de sensores conoceremos el tiempo que ha pasado que siempre es el
mismo, es el tiempo de la interrupción que configuremos en el
timer, por lo que la derivada del error pasa a ser error_anterior
-error.
Configurar un timer con una interrupción que llame a una función cada x tiempo son unas pocas líneas de código:
void inicializar_timer1(void) //Configura el timer y la interrupción.
{
OCR1A= 0x0C35;
TCCR1B
|=((1<<WGM12)|(1<<CS11)|(1<<CS10));
//Los bits que no se tocan a 0 por defecto
TIMSK1 |= (1<<OCIE1A);
sei();
}
Si añadimos esto a los programas anteriores se configura el
timer1 de la Baby Orangutan, para que cada 10 ms se active una
interrupción que llame a una función para que le atienda
. Cuando se produce la interrupción el micro ejecuta el
código que le indiquemos, más información sobre
las interrupciones en los AVR y WinAVR aquí.
Por lo que para probar que está bien configurado el timer1 de la
baby Orangutan escribimos el siguiente código para que se
ejecute cuando se active la interrupción:
Para trabajar con las interrupciones del WinAVR debemos poner el
siguiente include al principio del programa: #include
<avr/interrupt.h>.
Cada vez que el timer alcance la cuenta que le hemos indicado en el
registro OCR1A (10 ms) el micro enciende el led rojo de la placa de
usuario, cuenta 5 ms y lo apaga, conectando el osciloscopio al led
podemos comprobar que el timer está correctamente configurado. Código completo.
Se puede ver como cada 10 ms se ejecuta lo que hay dentro de
ISR(TIMER1_COMPA_vect) y funciona como se había esperado. Si en
lugar de encender el led, leemos la placa de sensores para saber el
error, esta lectura se realizará cada 10 ms, por lo que restando
el error medido al anterior podemos conocer la velocidad de cambio de
éste, ya que el tiempo es constante entre todas las medidas. Si
estamos leyendo sensores en analógico o usamos la placa de
sensores de condensador este cálculo es directo, (error -
error_anterior) nos dará la derivada del error ya que el error
se puede considerar continuo, tenemos la medida real de la
distancia del sensor a la línea y el tiempo de muestreo es
constante.
Si usamos sensores digitales no lo podemos hacer de una manera tan
directa, ya que sólo sabemos si el sensor está a 1
ó a 0, el error lo tenemos en pasos equivalentes a la distancia
entre sensores, por lo que no tenemos una derivada si no que buscamos
la velocidad media de cambio del error entre dos sensores. Por lo que
una forma de tener una medida precisa de la velocidad del error es
medir el tiempo que tardan los sensores en cambiar, cuanto tiempo
transcurre desde que se activa un sensor hasta que se activa otro.
Conociendo la distancia entre sensores que es fija e igual para todos y
el tiempo podemos conocer la velocidad media de desplazamiento entre
sensores de la línea bajo la placa de sensores.
Por lo que una función para determinar la derivada/velocidad
media del error (distancia de la línea al centro de la placa de
sensores) podría ser la siguiente:
int obtener_errord(void)
{
int error = 0;
static int error_old = 0;
static int errord=0;
static int errord_old = 0;
static int tic = 1;
static int tic_old = 1;
//Cálculo de la velocidad media del error.
if (error == error_old)
{
tic = tic + 1;
if(tic > 30000)
tic = 30000;
if(tic > tic_old)
errord = errord_old/tic;
}
else
{
diferencia = error - error_old;
errord = Kd*(diferencia)/tic; //error medio
errord_old = errord;
tic_old=tic;
tic=1;
}
error_old = error;
return(errord);
}
Se comienza leyendo los sensores
para determinar la posición de la línea, no se lee el
error medio de todos los sensores activados, ya que ésto debido
al grosor de la línea y al espacio entre sensores puede dar
lugar a errores, es decir, puede pasar de un error a otro (distancia)
con para distinto desplazamiento, lo que sería una medida
errónea de la velocidad. Leyendo los sensores de uno en uno del
centro hacía fuera y considerando que la línea
está en el sensor que se active antes evitamos el problema
anterior.
El cálculo de la velocidad media de cambio del error se hace aquí:
//Cálculo de la velocidad media del error.
if (error == error_old)
{
tic = tic + 1;
if(tic > 30000)
tic = 30000;
if(tic > tic_old)
errord = errord_old/tic;
}
else
{
diferencia = error - error_old;
errord = Kd*(diferencia)/tic; //error medio
errord_old = errord;
tic_old=tic;
tic=1;
}
error_old = error;
return(errord);
Lo primero decir que a esta
función la vamos a llamar a través de una
interrupción por lo que tic++ representa incrementos fijos de
tiempo. Una vez que hemos leído el error, la distancia de la
línea a la placa de sensores, se compara con el error anterior,
si es igual incrementamos la cuenta de tiempo y se termina la
función. Si no es igual al anterior se compara de cuanto ha sido
el cambio, es decir cuantos sensores se ha desplazado la línea,
diferencia = error - error_old; y se cálcula la velocidad errord
= Kd*diferencia/tic , es como calcular la velocidad, nada más
que al espacio lo llamamos error (distancia de la línea a la
placa de sensores), Kd es una constante que se usa para ajustar el
regulador y tic lleva la cuenta desde la última vez que se
produjo un cambio, es decir, cuanto tiempo ha tardado la línea
de pasar de un sensor a otro. Calculado el error la función lo
devuelve.
Cuando no cambia el error está función puede devolver dos
cosas, el errord (valor devuelto por la función) calculado
cuando cambio, y otro errord menor. Para ello cuando cambia el error y
se cálcula la velocidad de cambio también se guarda el
tiempo que tardó en cambiar (aunque va implícito en el
anterior), esa velocidad se deja fija en el siguiente intervalo y se
resetea la cuenta de tics. Si la cuenta de tics excede la cuenta de
tics del cálculo anteior significa que ha pasado más
tiempo y el error aún no ha cambiado, por lo que se empieza a
calcular la velocidad de cambio el error según va pasando
más tiempo.
Aunque puede parece un poco lío es calcular el espacio y la
velocidad, velocidad = espacio/tiempo, nada más que a espacio lo
llamamo error. Conociendo la velocidad podemos anticipar la respuesta
del robot y evitar las oscilaciones.
Por ejemplo leemos el sensor D2 y un tiempo después leemos el
sensor D3, lo que significa que la línea se está
desplazando de izquierda a derecha. Si aplicamos la fórmula errord = Kd*(error - error_old)/tic; nosqueda
errord = Kd*(7-5)/t, un errord positivo, si este errord es muy grande
le dice al robot que gire más hacía la izquierda que en
el próximo instante de tiempo la línea va a estar
más alaejada del centro, por lo que nos anticipamos a lo que va
a suceder.
Ahora por ejemplo leemos D0 y el anterior sensor leído fue D1,
errord = Kd*(1 - 3)/t, da un errord negativo que indica al robor que la
línea se está desplazando hacía la izquiera, si
este errord es lo suficientemente grande el robot empezará a
"enderezar" la dirección antes de llegar al centro de la placa
de sensores y pasarnos, metiendonos en la zona de los sensores I y
empezando a oscilar sobre la línea, como ocurre sólo con
el regulador proporcional cuando no miramos la derivada para
anticiparnos a lo que va a suceder.
La función de error proporcional es la misma que la anterior:
int obtener_errorp(void)
{
char errorp=0;
static char ultimo_errorp=0;
char contador_sensor=0;
Devuelve el error de la línea
al centro de la placa de sensores que multiplicamos por una constante
para ajustar el regulador.
El regulador y control de los motores lo implementamos en la ISR del
timer1, para que tenga lugar con un tiempo fijo, necesario para poder
hacer la derivada o velocidad media.
ISR(TIMER1_COMPA_vect)
{
PORTD |=(1<<LEDP);
int errort=0;
int proporcional = obtener_errorp();
int derivativo = obtener_errord();
Comenzamos llamando a las funciones
anteriores que nos devuelve el error proporcional y la velocidad media
del error (espacio y velocidad), el error total es la suma de los dos,
el valor de los errores obtenidos por las funciones ya van
multiplicados por dos constantes, Kp y Kd, que utilizamos para ajustar
el comportamiento del error, aumentandolas o disminuyendolas hacemos
que cada uno de los errores tenga más o menos peso en el
resutado final. Puede parecer un poco confuso pero una vez visto la
idea es muy sencilla.
Una vez que tenemos el error total pasamos a ajustar la velocidad de
cada motor, sí éste es mayor que cero giramos a la
izquierda y si no a la derecha, cuando el motor de un lado va
más deprisa que el del otro se enciende su led, cuando ambos van
a la misma velocidad los dos leds se apagan.
Al principio de la ISR encendemos el led de la placa y al final se
apaga, de esta forma tenemos una manera rápida de comprobar el
tiempo de ejecución del código sin tener que usar un
osciloscopio, si el led brilla poco nos sobre tiempo (en este programa
casi no brilla)m y si brilla mucho mala señal. Ésto es
importante porque estamos haciendo divisiones y para estos micros eso
puede llevar mucho tiempo, por lo que hay que comprobar que para el
tiempo que fijemos el código que escribimos tiene tiempo de
ejecutarse.
El programa completo del regulador del robot para verlo funcionar sobre la línea está aquí.
Al principio del programa encontramos lo siguiente: /*********** Ajuste comportamiento robot *********/
//Constantes Regulador PD.
int Kp=10;
int Kd=65;
int velocidad = 100; //Máxima 255.
/*************************************************/
Sirven para ajustar el
comportamiento del robot, para ello se establece una velocidad media y
se va incrementando la Kp para que el robot siga la línea sin
salirse, una vez hecho ésto se va incrementando la Kd hasta que
el robot siga la línea sin balancear. Llegados a este punto
aumentamos la velocidad y repetimos el procesos desde los valores de Kp
y Kd que teníamos.
Para ver la importancia de estas constantes y como realmente la
velocidad media del error vale para anticiparse y que el robot no
oscile, un video:
Comenzamos con el valor de Kd a cero, es decir, sólo tenemos
error proporcional y pasamos de la derivada y como se ve en el video el
robot no pasa de oscilar. Los leds que indican que un motor gira
más rápido que el otro sólo se encienden cuando la
línea está en el lado opuesto, por lo que es muy
difícil (por no decir imposible) que se pueda seguir sin entrar
en oscilación. Luego probamos a ir aumentando la Kd y le damos
el valor 65 para el mismo valor de Kp y velocidad anterior, y como se
puede ver la respuesta del robot es muy distinta, hemos dejado de
oscilar, también se puede apreciar como se encienden y se apagan
los leds de los motores cuando la línea está a un mismo
lado de la placa de sensores, lo que significa que el error derivativo
es mayor y nos anticipamos a lo que viene.
Para ver más o menos la velocidad del robot pongo otro video,
las constantes no están ajustadas, sólo se ha probado dos
o tres valores para ver lo que salía.
El resultado es bueno, en el segundo corte que es el que más
velocidad tiene de los dos, la velocidad está en 220 de 255, y
creo que ya pasamos el metro por segundo. El regulador no está
ajustado, he puesto los valores que me parecían 2 ó 3
veces por lo que todavía se puede mejorar la respuesta, se
observa como el robot oscila y se para un poco. No sé
cuál es la medida de la pista para calcular la velocidad, pero
después del primer corte cuento que el robot da 4 vueltas en
algo menos de 30 segundos, es decir 7.3 s por vuelta
aproximadamente.
Y comparo con este otro video en la misma pista:
Son los MiniZ del año pasado del Cosmobot y al más
rápido de los dos le cuento las 4 primeras vueltas a 8.2 s por
vuelta, y si no recuerdo mal el año pasado quedó cuarto
en la clasificación, no recuerdo la velocidad. Por lo que mala
noticia para el MiniZ 2009, un robot de bajo coste y sin ajustar ya le
ha superado =(
Bueno lo básico para tener el robot funcionando ya se ha hecho,
falta ajustar el robot y ponerle la velocidad al máximo para ver
el resultado final, cosa que haré en los próximos
días/semanas en una pista grande, sin baches.., y que tengo
medida. Además de ajustar el regulador queda hacer el
código para los interruptores e implementar el ADC para leer la
batería, pero es menos importante, la parte que iba a determinar
si el robot valía para el metro por segundo o no, era
implementar el este regulador. Los sensores se leen en digital y
si no hay luz ambiente (aunque ésta también se carga a
los analógicos si los satura) pues creo que no habrá
problemas, pero habría que hacer pruebas con distintas
iluminaciones.
El código sólo está escrito y visto que funciona,
no lo he repasado por lo que podrá tener algún error y se
puede mejorar, lo he hecho según lo que he ido pensando y parece
que funciona así que lo dejo así.
En principio el primer resultado del robot parece bastante bueno, por
lo que para ser el primer velocista diferencial que hago y con las
limitaciones de las piezas elegidas pues estoy contento. Queda ajustar
el regulador pero no sé cuando lo haré porque tengo que
dedicar tiempo a otros proyectos. Lo que queda demostrasdo es que un
robot velocista de bajo coste para presentarse a un concurso y no
quedar de los últimos se puede hacer en dos o tres de fines de
semana y no es tan difícil, este ha sido mi primer velocista
diferencial por lo que he tenido que pensar casi todo desde cero, y me
he pasado más tiempo escribiendo que haciendo...
Para cualquier duda sobre todo ésto, idea, etc... : foro.
