En el tema
anterior construimos una gráficas mostrando la relación entre el input (% PWM)
aplicado y el resultado (velocidad) obtenido por el motor. Podríamos usar esa
información para controlar nuestro motor. Si queremos que gire con una cierta
velocidad, podríamos ver que entrada (PWM) precisamos y aplicarla. Sin embargo
ya comentamos que dicha relación cambia por numerosos factores, por lo que
nunca estaríamos seguros de alcanzar la velocidad deseada.
Técnicamente
esta situación se conoce como un control de bucle abierto, al no tener un
feedback que nos informe de lo que realmente está haciendo el motor. De hecho
disponemos de dicho feedback (la info de los encoders en cuadratura) pero hasta
ahora no la estamos usando.
En esta
entrada vamos a "cerrar" el bucle, usando el feedback de que
disponemos para controlar la posición de un
motor. La entrada suministrada por el usuario no será un PWM (de efectos
inciertos) sino un objetivo en términos de la velocidad o posición deseada del
motor. El sistema, en función del feedback recibido, variará adecuadamente su
variable de control (en este caso el ciclo ON del PWM) para conseguir el
objetivo dado. Usaremos un sencillo controlador PID para la implementación.
Al igual
que en el tema anterior, acompañaremos al código en C del microcontrolador con
un programa en MATLAB para poder mostrar de forma gráfica los movimientos del motor.
PID_pos.c monitor_PID.m (MATLAB)
-----------------------------------------------------------------------------------------------------------
Controlador PID de posición.
En
aplicaciones de control el primer paso es identificar la variable que queremos
controlar y la variable que podemos modificar. En un ejemplo típico, podríamos
querer controlar la temperatura de un acuario y para ello podemos actuar sobre
el tiempo que un calentador está encendido.
En nuestro
caso la variable a controlar será la posición del motor y la variable de
actuación será el duty del PWM. Esencialmente estamos interesados en convertir
nuestro motor DC en un servomotor, al que le podamos decir a que posición
queremos que vaya.
La idea de
cualquier sistema de control es poder calcular una discrepancia (error) entre
el objetivo (posición deseada, introducida por el usuario) y la realidad
(posición del motor obtenida a través de los encoders). Dicho error se usará
como feedback para modificar la variable de control (PWM en nuestro caso).
Un
controlador PID es un tipo de control muy usado en la práctica por su
sencillez, dado que no requiere una caracterización completa del sistema. Se
basa en combinar 3 términos (Proporcional, Integral y Diferencial) relacionados con el error con unas constantes
que se pueden determinar a través de un proceso de prueba y error.
En nuestro
caso, siendo Pref la posición deseada (target u objetivo) y siendo pos
(determinada por los encoders) la posición real del motor, definiremos el error
en un momento dado como
A partir
de este error, modificaremos nuestra variable de actuación en base a la
siguiente expresión:
Por el
momento podemos considerar que pid es simplemente un valor que, en caso de ser
positivo, indica que el motor debe girar hacia delante y si es negativo, hacia
atrás. Nosotros nos ocuparíamos de convertirlo a un valor p.e. entre 0 y 1023
como vimos en el tema anterior para aplicarlo al PWM (o entre -1023 y 1023 si
usamos el montaje A).
En la
fórmula, el primer término es proporcional al error, el segundo proporcional a
su integral y el tercero va con su derivada.
La idea es
que el primer término trata de reducir el error actual. Si el error es positivo
(no hemos llegado a Pref), pid es positivo, por lo que aumenta la posición y se
reduce el error. Si el error es negativo (nos hemos pasado), se invierte el
duty y nos movemos en sentido contrario, acercándonos de nuevo al objetivo.
El segundo
término refleja la influencia del "pasado" del error: nuestra
actuación no será la misma si puntualmente tenemos un cierto error que si llevamos un "rato" largo con
dicho error. En el segundo caso la actuación será más enérgica.
Finalmente
el tercer término refleja el "futuro" del error (su tendencia,
capturada en su derivada). Imaginad que estamos a 5 unidades de nuestra
posición deseada, o lo que es lo mismo, tenemos un error e=5. No es lo mismo si
nos estamos acercando al objetivo (se está reduciendo el error, derivada
negativa) que si nos estamos alejando (derivada positiva, el error está
aumentando). En el primer caso a lo mejor no hay que hacer casi nada (la propia
inercia del movimiento nos lleva al objetivo, mientras que en el otro caso está
claro que hay que corregir la situación.
En nuestro
caso en vez del continuo antes descrito tendremos una implementación discreta.
Cada cierto tiempo (en la rutina del servo de la que hemos hablado en temas
anteriores) se mide la posición actual (encoders) y se calcula un término de
error:
Para
calcular el término de control, el error instantáneo se traduce de forma
natural en el error medido en el instante n, pero para la parte integral y
derivativa tendremos que hacer aproximaciones numéricas:
- La integral se substituye por una suma de valores anteriores del error. Además se limita en el tiempo, sumando los últimos N errores, en vez del historial completo de errores. Formalmente tendríamos que añadir un término h (el intervalo de tiempo entre muestras) pero dicho término (constante) puede ser absorbido por la constante de integración Ki.
- En cuanto a la estimación numérica de la derivada, lo más inmediato es substituirla por la diferencia entre errores sucesivos, aunque pueden usarse estimaciones más exactas. De nuevo, el factor h que aparece puede entrar dentro de la constante Kd, por lo que no es necesario introducirlo explícitamente:
En su
variante discreta el término de control a calcular para actuar sobre el PWM del
motor será por tanto:
Indicar por
último que no es necesario que la frecuencia de toma de medidas sea la misma
que la frecuencia de actuación del servo. Es posible, p.e. tomar medidas con
una frecuencia de 100 Hz pero solo actuar sobre el motor 20 veces por segundo
(20 Hz). En estos ejemplos por simplicidad usaremos la misma frecuencia para ambos,
por lo que tras tomar cada medida, calcularemos el término de control y
modificaremos el ciclo ON del PWM.
Implementación (PID_pos.c)
Pasemos a la implementación. La mayor parte del código
ya la hemos visto en las entradas anteriores. Por ejemplo, la parte de la interrupción de
alta prioridad INT0 para contar los pulsos de los encoders es la misma, así
como el uso de la interrupción del TMR2 para la rutina del servo.
La diferencia
es que la rutina del servo antes sólo actualizaba la posición del motor (a
partir de la lectura de los encoders). Ahora, a partir de dicha posición y del
objetivo fijado tendremos que calcular el término de actuación anterior
(pid) y traducirlo a nuestro esquema particular. Por ejemplo, un pid = 0 indica
que todo esta bien y no hay que hacer nada. Eso equivale a ordenar un PWM de
512 (50%) si hemos elegido un montaje de tipo B (ver entrada anterior).
Veamos con
más detalle los añadidos a la rutina del servo (interrupción TMR2)
if (TMR2_flag)
{
cont_servo++;
if ((cont_servo&0x0F)==0)
{
temp=quad; vel=temp-last_quad; //
delta = delta counter
last_quad=temp;
if (vel>128) vel-=256; // correct for rollover
else if (vel<-128) vel+=256;
pos=pos+vel; //
increment position
error=Pref-pos; // Current error
if (err_idx==0) last=past[NPAST-1]; else
last=past[err_idx-1];
error_der = (error - last); // Derivative error
// Acumulated error
error_int-=past[err_idx]; error_int+=error;
past[err_idx]=error;
err_idx++;
err_idx&=(NPAST-1);
compute_pid(); // combine PID terms
create_msg(); send_msg(msg,12); // Send data through serial port
}
TMR2_flag=0; return;
}
La primera
parte es idéntica a la que ya conocemos. La rutina del servo se ejecuta cada
cierto número de interrupciones (en este caso, 16, que junto con un POSTscaler
de 16 para TMR2 nos da una frecuencia de unos 75 Hz para el servo). Para ello
usamos el contador cont_servo.
La parte
de calcular el incremento de posición es también idéntica. Corregimos posibles rollovers de la variable quad e incrementamos la posición. A partir de aquí empezamos a hacer cosas nuevas.
A partir
de un objetivo establecido para la posición (Pref) se calcula:
- Error instantáneo (error) para la posición de ese momento (error instantáneo).
- Estimación de la derivada del error (error_der) restando errores sucesivos.
- Error integral acumulado (error_int).
Para el cálculo del error integral o acumulado se guardan
los NPAST últimos errores en un buffer circular donde van siendo machacados según
llegan datos nuevos. El error integral es simplemente la suma de los contenidos del buffer en un momento dado. Para optimizar su cálculo, lo que
hacemos es restar de la suma total el error más antiguo (antes de machacarlo) y
sumar el nuevo error (que machaca al más antiguo). Así evitamos tener que hacer una suma de todos los datos cada vez. Tanto el buffer como su suma total se inicializan a 0 al
empezar.
// Error
integral
error_int-=past[err_idx]; error_int+=error;
past[err_idx]=error;
err_idx++;
err_idx&=(NPAST-1);
Tras
almacenar el error más reciente se incrementa el cursor (err_idx) del buffer
circular de antiguos errores (past). En este ejemplo estamos usando un tamaño
NPAST=32 para el buffer del "pasado". Aproximadamente conservamos
algo menos de ½ segundo (32 muestras a 75 Hz) en nuestro buffer.
Finalmente,
una vez conocidos el error instantáneo, error integral (suma del buffer del
pasado) y error derivativo tenemos todo lo
necesario para saber como actuar. La rutina compute_pid() hace exactamente eso:
combina los valores de los tres términos con las tres constantes (guardadas en
un array K global), obtiene el término de control y lo convierte a un valor del
"duty cicle" del PWM que programamos en el correspondiente registro
del PIC:
void compute_pid(void)
{
float ee,ed,ei;
float pid;
ee = error; // Current error
ed = error_der; // Derivative error
ei= error_int/NPAST; // Integral error
pid = K[0]*ee + K[1]*ei + K[2]*ed; // P,I,D terms
if
(pid>=1.0) pid=1.0; else if (pid<=-1.0) pid=-1.0; //
pid in -1..1
pid =512+500*pid;
set_pwm1((uint16)pid);
return;
}
De nuevo
podemos elegir entre las distintas opciones discutidas en el tema anterior
sobre como conectar las líneas de control al driver del motor. Como hemos usado
el motor Maxon GM-20 para las pruebas hemos optado por el montaje (B) que
mostraba un comportamiento muy lineal entre PWM y velocidad. Por lo tanto el valor de control, entre -1 y
1 se translada al intervalo [0, 1023], de forma que un valor de control 0 vaya
a parar a 512 (50% PWM), que corresponde a mantener el motor en reposo
(nominalmente).
Como vimos
en el tema anterior diferentes combinaciones de motores/drivers pueden mostrar
peculiaridades propias que hagan
necesarias algunas correcciones a la traducción entre variable de control
(-1,1) y PWM. Por ejemplo, para esta combinación de driver L293D y motor GM20
detectamos una pequeña zona muerta ("deadband") en la zona entre
PWM=500 y 520. Podríamos compensarla en "software" de la siguiente
forma:
pid=pid*500;
if (pid>0) pid+=520; else if (pid<0) pid+=500; else pid=512;
De esta
forma, el mínimo PWM que se adjudica a un pid no nulo es de 520, lo que asegura
que el motor se moverá algo. De la otra forma un error pequeño podría darnos un
PWM de p.e. 515. Dicho PWM, aunque nominalmente debería mover el motor, en la
práctica no tiene ningún efecto, por lo que el error no se reduciría.
Finalmente,
al final del proceso vemos un par de rutinas create_msg y send_msg(). Lo que
hacen estas rutinas es construir un pequeño paquete con los datos más
relevantes y enviarlo por el puerto serie. El paquete usado sigue el esquema
explicado en el tema anterior: empieza con el carácter 'A', sigue con unos
cuantos datos (posición, velocidad, objetivo y PWM duty) y termina con un
contador de mensajes y el carácter 'Z' cerrando el paquete.
void create_msg(void)
{
int16 temp;
uint8 k,*ptr;
msg[0]='A';
ptr=(uint8*)&pos; for(k=0;k<4;k++) msg[k+1]=ptr[k];
msg[5]=vel;
temp=(int16)Pref;
ptr=(uint8*)&temp; for(k=0;k<2;k++)
msg[k+6]=ptr[k];
ptr=(uint8*)&duty; for(k=0;k<2;k++)
msg[k+8]=ptr[k];
msg[10]=msg_cont; msg_cont++;
msg[11]='Z';
}
void send_msg(uint8* msg,uint8 n)
{
uint8 k;
for(k=0;k<n;k++) { tx_buf[tx_next]=msg[k];
inc(tx_next);}
enable_TX_int;
}
Tras crear
el paquete la segunda rutina lo manda por el puerto serie. Como aquí podríamos
estar mandando un volumen grande de datos hemos usado el enfoque de transmisión
de datos a través de la UART usando un buffer de transmisión e interrupciones.
Por lo tanto la segunda rutina no hace más que copiar los bytes del mensaje al
buffer de salida y activar la interrupción TX. El código de dicha interrupción
irá vaciando dicho buffer. En previsión de si aumentamos el número de datos o
la frecuencia con la que los mandamos configuraremos el puerto serie a 115200
baudios.
Interfaz con el usuario
Para
comprobar nuestro código de control podríamos haber hecho algo similar a lo que hicimos para el caso de variar el PWM: implementar un sencillo menú donde, a través del puerto serie, el usuario pudiera dar ordenes
del tipo de: ve a la posición 2000, vuelve a la posición 0, etc.
Sin
embargo, como se trata de un programa ilustrativo, y nuestro objetivo en este entrada es entender un poco mejor como funciona un controlador PID, lo que he hecho es que el
usuario puede modificar las constantes Kp, Kd y Ki desde el PC (a través del
programa de MATLAB monitor_PID), de forma que pueda tener una idea del proceso de ajustar los parámetros de un PID.
Dentro del programa en el microcontrolador, partimos de un objetivo Pref=0 como "target" para la posición. Cada cierto tiempo (2/3 segundos), dicho objetivo cambia, alternando entre
0 y 500 (500 ticks con este motor en modo 2X corresponden aproximadamente a 1/4 de vuelta) . En un principio las tres constantes Kp, Ki y Kd están a 0, por lo que a pesar de los posibles errores no estamos actuando sobre el motor.
A través de los botones de la GUI de MATLAB
podemos subir, bajar o resetear dichas constantes. Así podemos explorar el
efecto de los diferentes términos de un controlador PID en un problema sencillo
pero realista.
El
siguiente screencast muestra el manejo de la interfaz de MATLAB y el ajuste de
las constantes del PID hasta obtener un resultado aceptable. Remarcar que no
siempre es necesario usar los tres términos del PID. En este caso, p.e.
obtenemos un buen resultado sólo con la parte proporcional y derivativa.
Espectacular, sos uin maestro es lo que estaba buscando, te voy a seguir desde ahora, espero que tambien subas algo de lo que haces con matlab. Saludos de argentina capo.
ResponderEliminarEl interfaz para el usuario es en Guide, entonces los codigos que se muestran al final son para cada boton kp?
EliminarEs posible emplear las instrucciones de c18 en ccs??
ResponderEliminarLas funciones de las librerías de uno u otro compilador serán diferentes por lo que el código no es inmediatamente portable. Sin embargo, en general, los compiladores tienen una funcionalidad similar, por lo que lo normal es que exista una función equivalente en el otro compilador.
EliminarEl enfoque que trato de darle a estas páginas es presentar un enfoque que intente explicar como están funcionando por debajo las cosas, a nivel de los registros del PIC. De esta forma nos será sencillo portar nuestros proyectos a otros compiladores. O incluso a otros micros, ya que los conceptos básicos son comunes a diferentes familias de micros: todos tienen interrupciones, timers, ADC, algún tipo de PWM, etc.
Antonio
Gracias, la verdad la info presentada aqui es muy util y los conceptos son bien entendibles, una pregunta ¿sera que esto se pueda aplicar para controlar algo como un pendulo invertido?
EliminarPuesto que en lo que tengo entendido el pendulo invertido se basa en que el motor actue en base ala señal que le mande el encoder que seria la del angulo y asi intentar estabilizar la varilla, no se si estoy mal en ese pensar. Saludos desde México
Este comentario ha sido eliminado por el autor.
Eliminares cierto sera posible aplicar esto para el control de un pendulo inverso???
ResponderEliminarSi que se podría usar para controlar un péndulo invertido.
EliminarLo que sucede es que si el péndulo a controlar está directamente fijado al eje del motor no va a resultar muy vistoso (la varilla del péndulo estaría simplemente bloqueada en la posición correspondiente a la vertical).
Lo que tendrías que hacer para un proyecto más interesante es dejar libre la varilla del péndulo, montada sobre un carro o similar que tuviera un motor que permita moverlo hacia adelante o atrás.
Un sensor (puede ser un simple potenciometro conectado a la varilla o un acelerómetro) ) nos permitiría medir su desplazamiento respecto de la vertical. Dicha medida gobernaría un controlador PID aplicado al motor que intentase mantenerla vertical. La idea es que si se empieza a caerse hacia adelante muevas el carro (a través del motor) hacia adelante, intentando mantener el carro debajo del centro de gravedad.
Un saludo,
Anronio
me podrias mandar el circuito?
ResponderEliminarNo tengo circuito. La mayoría de estos ejemplos están montados sobre una protoboard. El esquema está explicado en la entrada anterior, sobre Manejo de motores usando PWM.
EliminarAntonio
buenos días desde colombia. agradeceria su comentario en mi duda. Estoy en este momento desarrollando un proyecto con unos motores paso a paso. de 1.8 grados. de paso. para cada paso que da debo hacer un sensado de luz. y determinar en que grado de los 180 posibles hay menor cantidad de luz. tengo el inconveniente de necesitar mas precisión ojala de 1 grado por paso. por lo que he optado por buscar información acerca de estos motores para ver si es posible realizar la misma función que un motor paso a paso de 1° de paso que no he conseguido. agradezco nuevamente su colaboración. gracias
ResponderEliminarSi lo que necesitas es más precisión en determinar la posición angular del mínimo a partir de una serie de medidas espaciadas 1.8º siempre puedes recurrir a interpolar tus datos.
EliminarEl proceso sería el siguiente:
1. Determinar por donde anda el mínimo. Por ejemplo puedes usar el valor mínimo obtenido en tus medidas, separadas 1.8º
2. Coger varios puntos (por ejemplo 3) que enmarquen el mínimo. Siguiendo con el ejemplo anterior usaríamos el que tiene el mínimo y 1 adicional a cada lado.
3. Hallar el polinomio de grado 2 que pasa por los tres datos. Sencillo de calcular usando una tabla de diferencias finitas (ver wikipedia)
4. Usar como dirección angular el mínimo del polinomio de interpolación.
Para fijar ideas, supón que tienes las siguientes medidas:
Angulo: Medida:
x-3.6 38
x-1.8 35
x 30
x+1.8 31
x+3.6 34
...
y 30 es el mínimo de tus medidas. Quieres determinar el mínimo con mayor precisión.
Si calculas el polinomio de interpolación para los 3 puntos centrales (x-1.8, x y x+1.8) y lo derivas para hallar el mínimo tendrás que este se sucede a 0.6º a la derecha de la medida central, esto es en x+0.6º.
Espero haberte servido de ayuda, Antonio.
no comprendo aún dos cosas, una es si la realimentación es interna en el micro??? y la otra es como juegas con el set point para variar la posición, en que pin va conectado el potenciometro para variar la posición???
ResponderEliminarSi. el feedback llega al micro a través de la lectura de los encoders en cuadratura. Dentro del programa se hacen los cálculos para reducir a cero el error.
EliminarEn cuanto a variar el setpoint en este proyecto, por ser de caracter ilustrativo, está preprogramado en el micro. Cada cierto tiempo alterno entre dos setpoints para ver como pasa el motor de uno a otro.
Sería trivial poner un potenciometro en uno de los pines y usar el conversor AD para leer su valor que se convertiría en el setpoint deseado.
Antonio.
Este comentario ha sido eliminado por el autor.
Eliminarlos enconder van conectados en RB0 y RB1, verdad??, que tipo de encoders usaste, por si acaso no fueron los TCST1103??
EliminarEste comentario ha sido eliminado por el autor.
ResponderEliminarEfectivamente están en los pines RB0 y RB1 que tienen interrupciones asociadas INT0,INT1 por cambio de estado. Consulta la entrada sobre codificadores en cuadratura para más detalles.
ResponderEliminarDe hecho si solo vas a usar el encoder en modo 2X podrías usar RB0 (con INT0) y cualquier otro pin declarado como una entrada digital, ya que no sería necesario asociar una interrupción a la segunda línea.
De esta forma, dado que este PIC tiene tres interrunciones INT0,1,2 podrías llegar a poder monitorizar 3 pares de encoders (tres motores).
Respecto a los encoders son los que venían integrados en los motores usados.
Antonio
Hola, antes que nada, quiero felicitarte, están muy bien hechos los tutoriales. Te escribo por que quiero saber si es posible que expliques como hiciste la GUI en Matlab, ya que he buscado en tus tutoriales y no he encontrado esa parte, en todos únicamente das la GUI ya hecha, pero no explicas cómo hacerla. Espero que puedas realizar un tutorial sobre cómo realizar la comunicación serial con Matlab, ya que en lo personal lo he intentado desde Simulink y no he podido hacer la comunicación bidireccional, por que truena la simulación, hacer el envío o la recepción por separado sí lo he hecho, pero a la hora de juntar ambas simulaciones en una sola, parece que se genera un problema con el puerto, se ejecuta un rato y después la simulación se detiene.
ResponderEliminarRecibe saludos, y nuevamente muchas felicidades.
Me apunto la sugerencia de documentar una GUI tipo en MATLAB con comunicación serie bidireccional.
EliminarYo no estoy usando Simulink, para la comunicación serie uso el comando serial de MATLAB para crear un objeto serie, al que luego puedo acceder para lectura y escritura.
Antonio.
Hola Antonio, muchas gracias por responder tan rápido. Estaré atento para cuando publiques la explicación de la GUI.
EliminarRecibe saludos.
Me podrias pasar el diagrama de implementacion para el Isis
ResponderEliminarHola, felicitaciones por el blog, está genial, quisiera saber si alguna vez has desarrollado algo de tus proyectos con metodos numéricos, gracias
ResponderEliminarHola soy Luciano, muchas gracias por ayudarnos a comprender estos conceptos de esta forma tan gráfica!.
ResponderEliminarEstoy tratando de copiar el ejercicio para asimilar bien los conceptos.
En principio el pic, el motor, el pid, el envio por puerto serie a matlab funciona, pero.... :(
no logro graficar en el Gui los datos del puerto serie en tiempo real, es decir con un eje x que no tenga limites.
solo puedo graficarlo con un numero n de muestras en el eje x y cuando lee las n muestras luego se para.....
Me podrias dar unas pistas de como abordar el grafico en tiempo real sin limite de tiempo en Gui?
Gracias por todo. Saludos!!!
Hola
ResponderEliminarMe llamo juan miguel y no entiendo esta linea de codigo: ei= error_int/NPAST;
¿Por que lo divides entre NPAST?
me podrías pasa el código del PIC
ResponderEliminarme podrías pasa el código del PIC
ResponderEliminarme podrías pasa el código del PIC
ResponderEliminaren ese link esta
ResponderEliminarhttp://artico.lma.fi.upm.es/numerico/antonio/blog/PID_pos.c
Uso matlab para mac ¿se puede conectar el PIC vía serial a una mac? Tengo muy poco conocimiento en PID.
ResponderEliminarHola gracias por tus aportes son excelentes. tengo una pregunta, si el error para el caso de un control de posicion es negativo , que influencia tiene en la senial actuante ?, aun mas profundo, como varia una senial actuante PWM si lo que se controla es la posicion de un motor dc?
ResponderEliminarLa señal positiva avanza en un sentido, mientras que si es negativa en otro. Depende de ti como interpretar la direccion y sentido.
Eliminarparcero sera que nos puedes brindar la información de que procedimiento se debe desarrollar para poder acceder a la información para poder hacer el PID, esta información seria(teta, set point ect), estos numero deben ser recolectado bien sea de manera física o simulada.
ResponderEliminarespero pronta respuesta
Hola... necesito hacer una práctica con comunicación spi y necesito que el maestro reciba el dato y el esclavo cuente de manera ascendente y descendente en un display de 7 segmentos, pero no tengo ni idea de cómo empezar. Todo esto en un pic 16f88. Alguien puede ayudarme?
ResponderEliminar