Programación velocista 1.    



Una vez que tenemos la electrónica del velocista viene la parte difícil y que más tiempo lleva, y es hacer un buen programa para alcanzar nuestro objetivo (seguir la línea lo más rápido posible). Lo primero para realizar el diseño del robot y su posterior programa es coger la normativa del concurso y ver con que nos vamos a encontrar, en este caso se toma como referencia la normativa del Cosmobot que es similar a la del resto de concursos que podemos encontrar por nuestro país. Esta normativa nos define el circuito en el que debemos correr, diciendo:

Artículo 2. Circuito
- El circuito será cerrado, de color blanco con guías fabricadas con cinta aislante.
- Existirán dos tipos de guías:
o De color negro: los robots pueden usar estas guías para navegar.
o De color rojo: los robots no pueden tocar estas guías, si lo hacen serán
descalificados.
- La pista estará formada por una sola calle de 15 ± 5 cm de anchura delimitada por dos guías
de color negro, de 2 ± 0.5 cm de anchura cada una, sobre una superficie clara.
- Los robots podrán seguir cualquiera de las dos líneas o navegar entre ambas. Se establecerá
unos límites de navegación interior y exterior a la pista, a una distancia mínima de 15 ± 5
cm de la misma, de modo que si alguna parte del robot alcanza estos límites será
descalificado de la carrera en la que esté compitiendo.
- El radio de curvatura de la pista siempre será superior a 40 cm, tomando dicha medida desde
el centro de la pista. La pista podrá tener curvas en diferentes sentidos.
- Las superficies de las pistas podrán presentar pequeñas irregularidades (sin tener que ser
perfectamente lisa) y la relación de refractividad entre las zonas claras de la superficie y las
oscuras será inferior a 0.5. Los sensores utilizados en los robots para detectar la pista
deberán poder reconfigurarse en situ para evitar posibles variaciones en las magnitudes
absolutas de sus parámetros puesto que la pista podrá estar iluminada con diferentes niveles
de intensidad luminosa, desde muy oscura hasta sobreiluminada (las pruebas podrían
celebrarse en entornos exteriores con luz solar).

Aquí hay que prestar atención a las dimensiones. La primera es la distancia entre las dos calles del circuito que es de 15 ± 5 cm, por lo general suele ser siempre de 15 o almenos así era el año pasado, no se hacía más ancha o más estrecha la pista intencionadamente jugando con ese margén de 10 cm, por lo que no nos encontrarémos tramos de 10 ó 20 cm de distancia entre las dos calles. Con esa idea inicial se parte y por tanto se va a considerar que la placa de sensores va a estar sólo sobre una línea (nunca sobre ambas a la vez ya que en este caso la placa de sensores tiene aproximadamente 11-12 cm entre los dos sensores más alejados, la versión final de la placa de sensores será menor porque su anchura está sobredimensionada para realizar las primeras pruebas) y por tanto nuestro error será la diferencia de la distancia del centro de la placa de sensores a la línea.

La otra distancia en la que pensar es en la anchura de la línea de 2 ± 0.5 cm, ésta nos dice el número de sensores que podemos tener activados a la vez bajo la línea, en el caso de esta placa nos encontraremos entre uno y tres sensores activados a la vez. La superficie del circuito será blanca, y aquí siempre se pone mucho cuidado en que no haya manchas que puedan proporcionarnos una lectura errónea. Llevando bien colocados los sensores y con unos buenos valores de resistencias no tendremos lecturas falsas producidas por los posibles focos o iluminación exterior, por lo que se puede asumir que siempre que estemos sobre la línea habrá entre uno y tres sensores consecutivos activos. El radio de curvatura nos define las dimensiones del coche que vamos a usar, un coche mayor de esta escala lo pasará mal para trazar esas curvas sin tocar la línea roja.

Estos aspectos son importantes a la hora de realizar el programa y se tendrán en cuenta más adelante, pero lo primero antes de empezar a programar es saber lo que tenemos y lo que buscamos, y un primer esquema sin perturbaciones no sé si acertado ya que me tengo que poner a repasar la teoría es algo así:




Bueno la primera idea para realizar el programa (espero que no muy equivocada) con la que vamos a empezar a trabajar para que Silent no quede último en el Cosmobot 2009 :P es la del dibujo: el microcontrolador se encarga de generar la señal de control, que depende de la posición deseada y de la posición actual. La posición deseada siempre va a ser que la línea esté debajo del centro de la placa de sensores, y en función de la posición actual asignaremos valores al servo y al motor para alcanzar el centro deseado. Esta señal de control es la entrada a un servomotor que se encarga de asignar el ángulo de giro y a un motor del que depende la velocidad, con los que obtendremos una corrección de nuestra salida (la posición actual) en el tiempo. Curse otra especialidad distinta a la de robótica y creo que mi regulación no paso de sistemas SISO y yo diría que éste es de dos entradas (señal de motor y servo) y una salida (posición), por lo que me va a tocar más que mirar por mi cuenta. Si alguien conoce un buen libro sobre el tema y otro que cubra las matemáticas que me deje un comentario pls.

Pero para empezar se va a asignar un valor constante a la velocidad (aunque el  objetivo final sería hacer su regulador al igual que el de posición), la velocidad tendrá un valor constante cuando el coche detecte que va recto (mediante un sensor trasero o transcurrido un tiempo con los sensores centrales a 1) y otro valor menor cuando los sensores no estén en el centro con el que habrá  menos problemas para recuperar la posición. De esta forma sólo tenemos que generar una señal de control para nuestro servo en función de la posición de la línea a velocidad constante. Una vez estudiado este punto y conseguido sus reguladores pues habrá que pensar en otro regulador para la velocidad, para el que habrá que crear otra electrónica para el coche, ya que la velocidad para un pwm dado del motor es función de la batería por lo que tenemos que incluir encoders (que está bastante difícil por el espacio disponible) o medir el valor de la batería si queremos saber a que velocidad vamos.

Otro aspecto muy importante y que me da miedo que sea un problema de estos coches son los tiempos, nosotros podemos mandar una señal a la electrónica de control del coche cada 16 ms, por lo que en principio sería nuestro tiempo de muestro de la salida. Para la parte del regulador derivativo puede que haya que muestrear más rápido.. En 16 ms a una velocidad máxima de 2.5 m/s (el Cosmobot pasado teníamos una velocidad media de 1.4 m/s con el coche hecho desde cero en una semana y sin tiempo para pensar, así que este año habría llegar por lo menos a 1.8 m/s de media, pongamos 2.5 m/s de máxima) se recorren 4 cm, por lo que cada 4 cm máximo podemos mandar una señal de control al coche para corregir la posición del servo, sumado al tiempo en el que el coche tarde en alcanzar dicha posición puede hacer que el sistema sea muy lento. Pero esto habrá que observarlo después de tener muy trabajado el programa y todas las posibilidades. Una posible alternativa para solucionar este problema es remover el microcontrolador que viene con el coche y poner en su lugar un 12f programado por nosotros que haga la misma función que el anterior pero con los tiempos que nosotros deseemos, claro que realizar ésto implica bastante trabajo.

Para poder avanzar hay que observar los resultados del programa que se mete en el micro, yo suelo grabar videos y luego verlos pero creo que no es suficiente. Por lo que lo suyo va a ser dotar al coche un módulo de comunicación inalámbrico para comunicar con el pc, que nos permita recibir los datos de los sensores en cada lectura y procesarlos. De esta forma podemos generar distintos trazados y ver como responde el coche según sus distintas variables, ya que todo ajuste de constantes se va a hacer de forma experimental. También sirve para agilizar el proceso de prueba, ya que se pueden mandar valores de forma directa al coche para sus constantes y se evita tener que estar reprogramando continuamente, por lo que este punto es casi obligado y cuanto antes se alcance mejor. Por lo que seleccionar un módulo y rediseñar la electrónica para utilizarlo pasa a ser prioritario.

Por último sensores en digital o en analógico, todo me hace pensar que acabaremos leyendo en analógico debido a una mayor precisión pero de momento vamos a darle unos intentos al digital para este año, lo del analógico para intentar no quedar últimos en el Cosmobot 2010.

Creo que este es un proyecto bastante entretenido y completo con el que se puede aprender un montón, y en el que yo me voy a centrar dedicando la mayor parte de mi tiempo y presupuesto robótico...

Bueno después de todas las ideas futuras anteriores, lo primero, para empezar a probar el coche y lo más simple es realizar el regulador proporcional que nos modifique la posición del servomotor (ángulo de giro) en función del error de la lectura de los sensores respecto al centro. Inicialmente vamos a dar una velocidad constante, y por la normativa, suponemos que sólo vamos a poder tener una de las dos líneas bajo la placa de los sensores y que siempre tendremos entre uno y tres sensores consecutivos activados al mismo tiempo. Basandome en el esqueleto del programa de timer2, un primer regulador proporcional sería el siguiente:.


//////////////////////////////////////////////////////////////////////////
// Primer programa para el AWD
// www.jmnlab.com
// febrero 2009
// Control proporcional, 16MHz
//////////////////////////////////////////////////////////////////////////

#include <p18f452.h>
#include <delays.h>
#include <timers.h>

#pragma config WDT=OFF, LVP=OFF, OSC=HS, OSCS=OFF, PWRT=ON, BOR=OFF, STVR=ON

void inicializar (void);
void timer2_isr (void);
void obtener_error(void);

#pragma code high_vector=0x08 // high interrupt vector en 0008h
void interrupt (void)
{
    _asm GOTO timer2_isr _endasm // Salta a la ISR
}
#pragma code


#define LED1 PORTEbits.RE0 // Ambar
#define LED2 PORTEbits.RE1 // Rojo
#define SENSORES PORTEbits.RE2 // Placa de sensores
#define MINIZ PORTBbits.RB4 // Control miniz

#define       R7 PORTCbits.RC0 //Extremo derecho
#define    R6 PORTCbits.RC1
#define    R5 PORTCbits.RC2
#define    R4 PORTCbits.RC3
#define    R3 PORTDbits.RD0
#define    R2 PORTDbits.RD1
#define    R1 PORTDbits.RD2
#define    R0 PORTDbits.RD3 // Centrales
#define    L0 PORTCbits.RC4 //
#define    L1 PORTCbits.RC5
#define    L2 PORTCbits.RC6
#define    L3 PORTCbits.RC7
#define    L4 PORTDbits.RD4
#define    L5 PORTDbits.RD5
#define    L6 PORTDbits.RD6
#define    L7 PORTDbits.RD7 // Extremo izquierdo

//Variables
unsigned char DelayCounter1;
int servo = 108, velocidad = 110;
int contador=0;
int errors=0;
char ultimosensor=3;


Lo primero como en todo programa los includes, declaraciones de funciones, los defines para reconocer de forma rápida que función tiene cada pata del pic y las variables. DelayCounter1 es la variable que utiliza la función de delay y que debemos salvar al llamar a la interrupción si usamos los mismos delays dentro de ésta. Servo y velocidad se corresponde con el ángulo de giro y con la velocidad del motor de nuestro coche, según se ha hecho los valores máximos son 160 y 60, siendo 110 el valor para el motor en parado y el servo centrado, éste último puede que haya que ajustarlo un poco mediante prueba y error  por la mecánica del coche para que vaya totalmente recto.

Sobre las variables los char ocupan un registro del micro 8 bits, y los int 2 registros del micro o 16 bits, por lo que siempre hay que usar el que necesitemos ya que la memoria no es que sobre. La variables están declaradas como globales, es decir el registro en todo el programa siempre está asignado a esa variable con la memoria reservada para ella, si las declaramos locales la memoria sólo es utilizada en la función en la que está declarada, quedandose libre para otras funciones cuando se sale de la función, por lo que también es otro aspecto a considerar cuando vamos pillados de memoria, pero en este programa vamos a usar muy poquita ram.

//Programa principal

void main (void)
    {
        inicializar();
        LED1 = 1;
        SENSORES=1;
        velocidad=101;

        while(1)
        {
            if(R7==1)
                ultimosensor=1;
            else if(L7==1)
                ultimosensor=0;
        }
    }

En la función principal empezamos llamando a inicializar() que se encarga de configurar el hardware del microcontrolador, encendemos un led y la placa de sensores (se puede apagar y encender durante la ejecución del programa por temas de consumo) y establecemos una velocidad constante para el motor del coche. La función while se ejecuta repetidamente y sólo se sale de ella mediante la interrupción del timer2 para leer los sensores, corregir las salidas y mandar la señal al micro interno. Se comprueba cuál ha sido el último sensor de los extremos leídos para que en el caso de que el coche pierda línea sepa hacía donde girar, ya que en principio leemos cada 16 ms y en este tiempo se pueden recorrer varios centímetros saltandonos el valor de giro máximo. Comprobando en el while estos sensores nos aseguramos que siempre que la línea pase por ellos los vamos a detectar ya que la frecuencia es de varios KHz, por lo que cuando perdamos la línea siempre podemos saber por donde ha salido y asignar el giro máximo sin saltarnos ningún sensor.

//******************************FUNCIONES*******************************************
void inicializar (void)
{
    TRISA = 0b11111111;      // Configuración de los puertos
    TRISB = 0b11101111;      //
    TRISC = 0b11111111;      //
    TRISD = 0b11111111;      //
    TRISE = 0b00000000;      // Puerto E como salidas.
    ADCON1 = 0b00001111;     // Todo puerto A como digitales.
    PORTB = 0;
    PORTE = 0;
    OpenTimer2 (TIMER_INT_ON & T2_PS_1_16 & T2_POST_1_16); //TMR2IF=0 TMR2IE=1 TMR2ON=1 TMR2=0
    RCONbits.IPEN = 0;
    INTCONbits.GIE = 1;
    INTCONbits.PEIE = 1;
    PR2 = 250;
}

Inicializar es una función para configurar el hardware del microcontrolador, comienza configurando los puertos del microcontrolador como entrada o salida mediante la instrucción tris (0 Output, 1 Input), ponemos los puertos como digitales ya que por defecto van como analógicos y configuramos la interrupción del timer2 y sus tiempos, explicado en timer2.

#pragma interrupt timer2_isr save=DelayCounter1  // interrupción, salvamos la variable de los delays

void timer2_isr ( void)  // 24 44 64 //60 110 160

{
   PIR1bits.TMR2IF = 0;  //Limpia el flag de interrupción del timer2
   obtener_error();

   MINIZ = 1;
   Delay100TCYx(16);      // 400 uS
   MINIZ = 0;
   Delay10TCYx(servo);
   Delay10TCYx(servo);
   Delay10TCYx(servo);
   Delay10TCYx(servo);
   MINIZ = 1;
   Delay100TCYx(16);      //400 uS
   MINIZ = 0;
   Delay10TCYx(velocidad);
   Delay10TCYx(velocidad);
   Delay10TCYx(velocidad);
   Delay10TCYx(velocidad); 
   MINIZ = 1;
   Delay100TCYx(16);  
   MINIZ = 0;  
}

La función que atiende a la interrupción se encarga de generar la señal requerida por el micro externo con los valores calculados en la llamada a obtener_error(), explicado en timer2. Se usan varios delays menores que en timer2 seguidamente, de esta forma podemos tener un margén mayor de valores que asignar a servo y motor para generar los tiempos si se quieren usar las funciones de delays que vienen en las librerias del compilador, ya que esta función tiene un unsigned char de argumento siendo 255 el valor máximo que se le puede pasar.

Por lo que tendremos que ajustar los valores producidos por nuestro regulador al argumento y tiempo producido por esta función. El valor de reposo para el servo y el motor usando este delay está en 110, los máximos en cada sentido en 60 y 160, con 50 de diferencia podemos asignar el pwm del motor y giro del servo en pasos del 2% del valor máximo en cada sentido, lo que en principio parece suficiente. Usando Delay100TCY salían unos valores de 24 44 64, reduciendo nuestros incrementos y decrementos a pasos del 5%, además de que cuanto más margen tengamos en el argumento de la función de delay mayor será el rango de constantes que podemos asignar al regulador.

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=108;
        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;

//Control proporcional.

        if (contador!=0)
            {
            LED2=0;
            servo = 108 - errors*3; //servo=centro-error*constante 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;
            }
        }
}

obtener_error() se encarga de leer los sensores, calcular la distancia de la línea al centro de la placa de sensores o error y calcular el giro de la dirección del coche, pero vamos por partes...

Aquí es donde aplicamos las dos suposiciones iniciales, el número posible de sensores activos va a ser menor de 3 todos ellos consecutivos y la placa de sensores sólo puede estar sobre una de las dos líneas por la fisíca de la pista y del coche en ambos casos, y ésto lo usamos para determinar cuál es el valor que debemos asignar a cada sensor activo, ya que debemos evitar las operaciones en coma flotante que requieren muchos ciclos de reloj para ser ejecutadas en estos micros, haciendo la ejecución de la interrupción mucho más lenta.

Si tengo 16 sensores, todos separados por la misma distancia, buscando como el punto a seguir el del medio entre los sensores 8 y 9 (L0 y R0), a esta posición le asigno el valor cero, hacía un lado de esa posición doy valores positivos y hacía el otro negativo.

L7 L6 L5 L4 L3 L2 L1 L0 R0 R1 R2 R3 R4 R5 R6 R7

Luego el error total será la suma de todos los sensores activos entre el número de sensores activados. El número de sensores activados es el que me determina si la operación va a acabar en decimales, es decir que el incremento que asigne a cada sensor debe de ser divisible entre los primos comprendidos entre el rango de sensores activados.

Por ejemplo si puedo tener hasta 3 ó 4 sensores activados, los incrementos deberían ser de seis (2*3), si puedo tener de 5 a 6, de 30, de 7 a 10 incrementos de 210. Pongo el caso de 4 que ya es un valor bastante por encima del grosor de la línea, tendría que dar incrementos de 6 al pasar de un sensor al siguiente.

L7  45
L6  39
L5  33
L4  27
L3  21
L2 15
L1  9
L0  3
R0 -3
R1  -9
R2  -15
R3  -21
R4  -27
R5  -33
R6  -39
R7  -45

Con estos valores al sumar y dividir por el número de sensores activados (restricción de máximo 4) siempre obtengo como resultado un número entero evitando las operaciones en coma flotante, pero esto con las funciones de delay de las librerias que usamos y el margen de valores que podemos asignar a su argumento no nos vale, ya que el valor máximo de error según esto puede ser de 45 y los valores del argumento de la función delay están entre 110 y 160, dejando 50 posibles valores. Por lo que si obtenemos un valor de error entre 0 y 45 no podemos jugar con la constante proporcional del regulador, Kp=1 ya abarcaría todos los posibles valores utilizables del argumento para generar el delay, por lo que hay que reducir el valor asignado al error a la vez que evitamos la coma flotante, y aquí es donde se usa la condición de que todos los sensores activados deben de ser consecutivos. Si los sensores son consecutivos se asignan los siguientes valores a cada sensor:

L7  15
L6  13
L5  11
L4  9
L3  7
L2 5
L1  3
L0  1
R0 -1
R1  -3
R2  -5
R3  -7
R4  -9
R5  -11
R6  -13
R7  -15

Con estos valores siempre vamos a obtener números enteros de error al dividir cuando el número de sensores consecutivos activados es menor de 4, si tengo 2 por ejemplo 5 y 7, resto al primero 1 y se lo sumo al segundo, tengo 6 y 6 que es lo mismo que 2*6 que entre dos sensores activos obtengo errors=6. Si tengo 3 sensores 3, 5, y 7, resto dos al último y se lo sumo al primero y me queda 5,5 y 5 que es lo mismo que 3*5 y entre 3 activos queda el errors=5, si tengo 4 sensores 3,5,7,9 resto 1 y 3 a los dos últimos y se lo sumo respectivamente a los primeros quedando 6,6,6,6 ó 4*6 que entre 4 sensores activos obtengo errors=4.

Con estos valores obtengo un error máximo de 15, lo cuál ya me permite utilizar más valores de Kp que pasar a la función de delay. Teniendo en cuenta que el número de sensores aprovechables estará por L4, L5, el error máximo proporcional sería 11 pudiendo usar una Kp de 5 para pasar al argumento dentro de los valores requeridos para el delay. Se pueden usar valores mayores para observar como se comporta en la práctica.

Al principio hay un if  con la condición de que ambos sensores centrales estén activos, cuando esto ocurre el servo se centra, esto es para evitar el error continuo que siempre tendríamos si la línea activase 3 sensores ya que cuando los dos sensores centrales están activos podemos considerar que tenemos la salida deseasa. La restricción de contador<3 está puesta para evitar manchas en la pista que pudieran darnos coma flotante al activar más sensores o pisar dos líneas, aunque se puede quitar y no pasaría nada.

            servo = 108 - errors*3; //servo=centro-error*constante proporcional

Esta línea es la que se encarga de asignar el valor que vamos a pasar al servomotor del coche, 108 sería el valor en el que la dirección está centrada para el coche, errors es la distancia a la que se encuentra la línea del punto medio (posición deseada) de la placa de sensores, y el número que multiplica a errors es la constante proporcional de la que depende la respuesta del coche/sistema para una velocidad dada. En la teoría se debe buscar que el sistema sea preciso, estable, rápido y adecuadamento amortiguado (que no se vaya a 20 cm de la línea y luego vuelva) y hay que hallar un compromiso entre estos 4 puntos, en nuestro caso debemos buscar que el coche no oscile sobre la línea ni toque la línea roja a la máxima velocidad posible. Cuanto menor sea el valor de Kp más lenta será la respuesta y cuanto mayor más rápida será la respuesta aumentando las posibilidades de oscilar volviendose inestable. Las K's de los reguladores se van a asignar de manera experimental sin una planta del coche, y esto es lo difícil acertar con ellas.

Después de esta instrucción hay un par de if para limitar la salia a valores entre 60 y 160, ya que un valor mayor o menor crearía un delay por encima o por debajo de los tiempos necesarios entendibles por el micro interno, teniendo una salida impredicible. Por lo que aumentando el valor de la Kp, los sensores que su error multiplicado por la constante exceda 50  pasan a quedar inutilizados, a efecto del regulador proporcional es como si no estuvieran.

Si en lugar de leer dentro de la interrupción y asignar el valor se hace fuera de ésta, que puede que sea lo más acertado ya que cuanto menos código haya en el ISR mejor, hay que salvar y recuperar los registros que estemos utilizando como W, Status ,etc.. para evitar resultados ipredecibles. Adjunto el código anterior en un .c.

A continuación unos videos del funcionamiento del coches sobre la pista para distintos valores de velocidad y distintas constantes de Kp, la cámara no se ve bienpero es lo que hay.

Funcionamiento del regulador proporcional.


Kp=1 Velocidad=101. 18% Vmax.


Kp=3 Velocidad=101.


Kp=5 Velocidad=101.


Kp=7 Velocidad=101.


Kp=3 Velocidad=99. 22% Vmax.


Kp=7 Velocidad=99.


 Como se puede ver en los videos, para un valor de velocidad 18% de la máxima:
 
Kp=1 la respuesta es demasiado lenta, además sólo alcanza el giro máximo cuando se sale de la línea debido a que hemos querido hacerlo así, por lo que acaba fuera del circuito. 
Kp=3 tenemos buena respuesta para esta velocidad con el coche sin salirse casi de la línea y poca oscilación. 
Kp=5 y Kp=7 vemos que el sistema se vuelve más rápido al aumentar el giro de las ruedas, lo que provoca que oscile mucho más.

Por lo que el mejor valor para Kp de los cuatro es el 3, suficientemente rápido para seguir la línea sin dejarla y con poca oscilación, pero si aumentamos la velocidad a un 22% de la máxima con este valor nos vamos fuera, somos demasiado lentos. Con un valor de 7 con la constante podemos seguir la línea pero con mucha oscilación.

Por lo que si queremos correr y debemos hacerlo hay que pensar en añadir una constante derivativa al regulador, que se encargue de medir la velocidad con la que cambia el error y en función de ésto corrija el valor mandando al servomotor, es decir un regulador PD. En este punto es donde nos quedamos el año pasado y el que denemos implementar este año si queremos mejorar en resultados, conseguir eliminar o disminuir la oscilación en recta para poder correr en ellas y combinarlo con la opción de frenar que llevan estos coches. No sé si conseguiremos hacer algo decente en el mes que queda ya que yo almenos me tengo que repasar toda la teoría de ésto que ando algo pérdido, pero por lo menos se intentará.

Para cualquier comentario, duda, corrección más abajo.. Gracias.



blog comments powered by Disqus
.