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
//////////////////////////////////////////////////////////////////////////
//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 (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.
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:
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.
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.