Programación Velocista 2.    



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))
        {

       if((L7==1)&&(contador<3))
            {
            errors+=15;
            contador++;
            }
        if((L6==1)&&(contador<3))
           {
            errors+=13;
           contador= contador+1;
            }
        if((L5==1)&&(contador<3))
            {
            errors+=11;
            contador= contador+1;
            }
        if((L4==1)&&(contador<3))
            {
            errors+=9;
            contador= contador+1;
            }
        if((L3==1)&&(contador<3))
            {
            errors+=7;
            contador= contador+1;
            }
        if((L2==1)&&(contador<3))
            {
            errors+=5;
            contador= contador+1;
            }
        if((L1==1)&&(contador<3))
            {
            errors+=3;
            contador= contador+1;
            }
        if((L0==1)&&(contador<3))
            {
            errors+=1;
            contador= contador+1;
            }
        if((R0==1)&&(contador<3))
            {
            errors+=(-1);
            contador= contador+1;
            }
        if((R1==1)&&(contador<3))
            {
            errors+=(-3);
            contador= contador+1;
            }
        if((R2==1)&&(contador<3))
            {
            errors+=(-5);
            contador= contador+1;
            }
        if((R3==1)&&(contador<3))
            {
            errors+=(-7);
            contador= contador+1;
            }

        if((R4==1)&&(contador<3))
            {
            errors+=(-9);
            contador= contador+1;
            }
        if((R5==1)&&(contador<3))
            {
            errors+=(-11);
            contador= contador+1;
            }
        if((R6==1)&&(contador<3))
            {
            errors+=(-13);
            contador= contador+1;
            }
        if((R7==1)&&(contador<3))
            {
            errors+=(-15);
            contador++;
            }

        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.

servo = 110 - errors*3; //proporcional Kp=3.


servo = 110 - errors*3 + errord*1; //proporcional derivativo Kp=3 Tic=8 Kd=1.


Y con un poco de prueba y error adiós a la oscilación en la recta. (Cambio de carril implementado).

Para cualquier comentario, idea, corrección... Gracias
blog comments powered by Disqus