Aquí va el primer intento de utilizar la derivada del error.... Una vez realizado el control proporcional (programación velocista 1)
lo siguiente es intentar medir la velocidad con la que cambia el error
y actuar en función de ello, es la primera vez que intento
realizar ésto así que cualquier consejo es bienvenido...
Para medir la velocidad con la que cambia el error hay que restar el
valor anterior del error al error presente y dividirlo entre un
intervalo de tiempo (Derror= (error_pasado - error_presente)/T), y
aquí el primero de los problemas, estimar el intervalo de tiempo
necesario. Leyendo los sensores en analógico ésto no es
tanto problema ya que tenemos un error continuo en todo el espacio por
lo que tendremos mucha más precisión para medir el error
para un tiempo dado. Al leer en digital nuestro error va en saltos, por
lo que si elegimos un tiempo muy pequeño la derivada del error
va a ser cero ó constante, tenemos mucha menos precisión
que en el caso anterior.
Por lo que una primera idea para resolver el problema de tener que
realizar la división con la coma flotante que
acarrea, puede ser contar los "tics" entre los que se produce un
cambio del error, es decir cuanto tarda la línea de pasar de un
sensor al siguiente que resulta mucho más preciso al poder usar
un tiempo de muestreo pequeño que de cierta forma podemos
considerar continuo frente a los saltos discretos de nuestros sensores.
El programa es el mismo que el anterior
sólo modificando la función obtener_error() para
añadir la derivada y declarando las variables necesarias, en
principio están declaradas la mayoría como int por
comodidad aunque con char y 8 bits hay más que suficiente, copio
la función modificada:
void obtener_error(void) { errors=0; contador=0;
//Asignar error, 3 sensores consecutivos, máximo activados, valores para obtener siempre entero en la división.
if ((L0==1)&&(R0==1)) servo=110; else if((L0==0)||(R0==0)) {
errors=errors/contador; //poner el if (contador !=0)
Hasta aquí igual que en el programa anterior. //Comprobar si ha cambiado el error y su derivada.
if (errors_pasado==errors) { tic--; if (tic<0) {// tic=0; errord=0; // no cambia, derivada = 0 para el regulador. }// } else if(errors_pasado != errors) { errord=(errors_pasado-errors)*tic; tic=9; } Lo
primero es comprobar si ha cambiado el error, para ello se declara la
variable errors_pasado, que toma el valor de errors al final de la
función por lo que comparandola con el errors actual se puede
saber si éste ha cambiado. Si son iguales significa que nada ha
cambiado y modificamos la variable que lleva la cuenta de los tics
(como se utiliza la ISR del timer2 estos se corresponden con 16
ms), si no son iguales el error ha cambiado y pasamos a calcular la
velocidad con la que este cambio ha ocurrido tras resetear la cuentas
de tic:
errord=(errors_pasado-errors)*tic;
errord es la variable donde se alamcena la velocidad de cambio del error.
errors_pasado - errors nos la magnitud y signo del cambio del error.
tic lleva el tiempo en el que ha ocurrido el cambio, y en lugar de
incrementarse se decrementan desde cierto valor ya que
multiplicamos en lugar de dividir para evitar floats, y si ha pasado un
tic entre el cambio la derivada debe ser mayor que cuando han ocurrido
dos. El valor de inicio habrá que calcularlo experimentalmente
ya que está relacionada con la constante derivativa.
Los posibles valores del error van de -15 a +15, yendo a la parte positiva y poniendo un ejemplo:
errors_pasado= 7
errors=8
tics=5
La diferencia entre errors_pasado sería -1, diciendonos que la
línea se desplaza hacía fuera del coche y que este cambio
del error se ha producio en 4 tics (64 ms), por lo que errord
sería igual a -1*5= -5. Si el cambio se hubiese producido
más rápido en un tic por ejemplo, la derivada
tendría que ser mayor por lo que errord sería -8 (tic=8),
si ha tardado más (6 tics) la derivada debería ser menor
-3. Si la cuenta sobrepasa los 9 tics considero que el tiempo ha sido
suficientemente grande y por lo tanto la derivada es lo
sufiicientemente pequeña para despreciarla y utilizar
sólo la parte proporcional en el regulador. En principio
había puesto 16 tic (0.25 sec) que en este tiempo a un par de
m/s pues salen unos 50 cm, si en esos 50 cm no ha hábido cambio
pues la derivada es totalmente despreciable. De manera experimental
para la velocidad constante a la que estoy probando he observado un
buen resultado para tics = 8.
Si el cambio en lugar de una unidad es de dos unidades, ya que nos
movemos muy rápido y en este tiempo de 16 ms se desplaza la
placa de sensores más de un sensor. El resultado si nos movemos
hacía fuera sería de -2, obteniendo por tanto una
derivada de -16 (-2*8 para un tic) correspondiendose conla derivada
buscada. Por lo que de esta forma contando tics podemos usar un tiempo
de muestreo pequeño que no nos valdría para la
división y los saltos en digital, obteniendo más
precisión en la velocidad del error.
Según lo anterior si nos sale un error de -5, más abajo tenemos la expresión:
servo = 110 - errors*3 + errord*1;
Por lo que
tendríamos servo = 110 - 8*3 -5*1; Nos estamos anticipando al
error futuro que se va a producir, ahora además de medir el
error de la posición de la línea sobre la placa de
sensores medimos la velocidad con la que ésta se desplaza y en
función de ello giramos más o menos el servo, si se
desplaza a mucha velocidad pues 16 ms más tarde la línea
estará prácticamente fuera de nuestra placa de sensores,
por lo que en lugar de corregir al máximo 16 ms más tarde
lo hacemos ahora, nos estamos anticipando.
Ahora el caso contrario:
errors_pasado=8
errors=7
tics=5
Aquí tendríamos un errord de +5, indicando que la
línea se está desplazando hacía el centro de la
placa de sensores, por lo que servo= 110 -7*3 + 5*1; Aquí lo que
decimos es que se está moviendo rápido hacía
dentro por lo que nos vamos anticipando en enderezar la
dirección para reducir esta velocidad, lo que evitaría
que nuestro coche entrase en oscilación al llegar al centro de
la línea con una velocidad alta de desplazamiento de la placa de
sensores, como pasa cuando se pretende ir un poco rápido
sólo con una constante proporcional
El *3 se corresponde con la constante proporcional y el *1 se
correspondería con la constante derivativa para probar distintos
valores sin tener que estar cambiando el valor de inicio de tic y el
valor de los decrementos de éste. Si el tiempo debe de ser
más pequeño para obtener mayor precisión pues
habrá que ajustar los resultados obtenidos ya que los valores a
pasar a los delays tienen 50 de margen en cada dirección y
habrá que incrementar el valor de inicio de tic.
//Control proporcional derivativo.
if (contador!=0) { LED2=0;
servo = 110 - errors*3 +
errord*1; //servo=centro-error*constante proporcional + derivada error*
constante derivativa // servo = 110 - errors*3; //proporcional if (servo>160) servo=160; if (servo<60) servo=60; } else if (contador==0) { LED2=1; if (ultimosensor==1) servo=160; else if(ultimosensor==0) servo=60; }
errors_pasado=errors; } }
Es una primera idea que parece que funciona, voy a pensarlo en hacerlo
así y si empezamos a quedar esta semana probamos y hacemos bien el código, aunque
también hay que ver como queda dividiendo directamente. Dejo los
videos de los resultados aunque he grabado los que menos se
aprecian.. ya que llevaba la Kp baja.