El p´endulo invertido es un problema cl´asico de control, se basa en mantener un polo en equilibrio opuesto al de un p´endulo normal es decir en contra de la gravedad. El sistema tendr´a como entrada un aceler´ometro y un giroscopio, ellos marcaran el ´angulo en el que se encuentra el p´endulo, con la informaci´on del ´
angulo que nos proporciona el sensor el algoritmo PID puede obtener el error de ´
angulo y tomar una acci´on de control. Como se menciono en secciones anteriores la primera implementaci´on del p´endulo, es decir con el filtro complementario tenia problemas, se obten´ıan bruscas vibraciones al intentar estar en equilibrio.
6.1.
Estructura
La estructura se compone por varios componentes, la base son dos laminas de madera, la inferior de 8 de ancho y 20 de largo y la superior 6,5 de ancho y 20 de largo, que se unen con dos esp´arragos de metal para que se puedan enroscar las tuercas por ambos lados y sujetar las bases una encima de otra. Encima de la base superior se ha hecho lo mismo con otro esparrago que en el lado superior del mismo se encuentra el sensor sobre una placa de prototipado. En la base inferior, por debajo tenemos los dos motores, uno a cada lado, en la parte superior de la misma base tenemos la placa del microcontrolador. En la placa superior se encuentra el puente en H donde se conectan los motores y a su lado la bater´ıa que los alimenta, en el lado opuesto de la misma base esta el bluetooth. La Figura 22 muestran fotos de la configuraci´on del p´endulo.
6.2.
Conexiones
Esta secci´on describe las interconexiones del dispositivo con sus perif´ericos. En la Figura 23 se muestra el esquema de conexiones del p´endulo.
Figura 23: Interconexiones del p´endulo
5 5 4 4 3 3 2 2 1 1 D D C C B B A A 3.3V 8V 5V MOTOR OUT IN MPU-9150 VCC GND SCL SDA EDA ECL ADO INT ZS404 STATE RXD TXD GND VCC EN dsPIC33FJ128MC804 SCL1 SDA1 PWM1L RP1 (RX) RP19 (TX) RB13 RB12 V D D G N D INT0 PWM2L 6.8K L298N ENA ENB IN1 IN2 IN3 IN4 12V 5V GND OUT1 OUT2 OUT3 OUT4 6.8K MOTOR OUT IN
En el centro del esquema se observa el dsPIC33F (con los pines no usados omitidos) y como est´an hechas las conexiones. A la izquierda se puede ver el chip L298N que es el puente en H, este esta conectado a dos pines de salida RB12 y RB13 que controlan la direcci´on de rotaci´on del motor, mientras que la lineas conectada a ENB y ENA son las que emiten la se˜nal PWM por separado. Estos est´an conectados internamente dentro del L298N a las salidas OUT que alimentan los motores desde la bater´ıa. En la parte superior derecha del esquema se puede ver como las lineas SDA y SCL se conectan a trav´es de resistencias de pull-up de 6.8KΩ. La linea marcada como ADO es la que determina el menor de los bits de la direcci´on I2C y al estar conectado a tierra la direcci´on acaba en un cero l´ogico, 0x67. Los pines RP19 y RP1 est´an remapeados a la transmisi´on y recepci´on de datos por serie con el modulo UART que se conectan al dispositivo Bluetooth ZS404. Cada chip se alimenta con diferentes voltajes, la alimentaci´on de la placa, el modulo bluetooth y el sensor vienen de 4 pilas de 1,5V en serie lo que nos proporciona 6V te´oricos, aunque la entrada llega a estabilizarse casi a 5V. El modulo bluetooth esta alimentado directamente desde las pilas en serie, mientras que la placa y los sensores pasan por un amplificador operacional para reducir el voltaje a 3.3V que es lo que admite la placa y el sensor. Los motores como ya se ha comentado en secciones anteriores viene alimentada por una bater´ıa de alta descarga, esta esta conectada al puerto que admite hasta 12V en el puente en H. El voltaje de salida de la bater´ıa se puede regular, esta emite 8V se ha optado por este voltaje para no sobrepasar el voltaje que pueden soportar los motores. En la Figura 24 se muestra el p´endulo desde varios ´angulos.
Figura 24: Componentes adicionales del p´endulo
6.3.
Algoritmo PID
En la teor´ıa de control un controlador es un dispositivo que monitora y al- tera la condiciones operacionales de un sistema din´amico. Ejemplos cl´asicos del control pueden ser controla la temperatura dentro de un espacio como un refri- gerador, controlar la velocidad de giro de una turbina hidr´aulica, etc. El nombre viene dado por sus variables en ingles ”Proportional-Integrative-Derivative Con- troller”, cada una asiste al controlador de manera diferente. La parte proporcio- nal como su nombre indica es directamente proporcional al error en un instante dado, cuanto mas aumente este par´ametro los cambios en la salida ser´an mas grandes y llega a desestabilizar el sistema. En sistemas como control de veloci- dad solo modificando la parte proporcional no es suficiente para llegar al valor deseado, aqu´ı es donde entra la parte integrativa, esta puede ser definida en un rango temporal, saturada o total. La parte integrativa suma el error cometido durante un tiempo dado del algoritmo y al multiplicada por la constante Ki es
la responsable de asegurar que la se˜nal de referencia y la se˜nal de salida llegan a coincidir, esto resolver´a el problema de la parte proporcional, pero a su vez intro- duce otro problema, denominado en ingles ¨Integrative Windup”que se resuelve saturando el resultado de dicha operaci´on. La parte integrativa tambi´en a˜nade un tiempo de respuesta mas reducido a la parte proporcional por lo que se llega antes al resultado deseado pero con una constante integral muy grande produce un efecto conocido como .Overshoot”. La parte derivativa se a˜nade para suavizar
la respuesta de la parte integrativa, esta intenta anticiparse al resultado siguien- te tomando datos de instantes anteriores, as´ı obteniendo la tendencia del error en el instante actual. A diferencia de las dem´as esta puede devolver un resultado negativo y este resultado negativo es lo que evita que exista un .Overshoot.enla
respuesta. La Formula 13 describe matem´aticamente la suma de las tres partes que es conocido como controlador PID. La Figura 25 muestra gr´aficamente el impacto de cada variable dependiendo del valor de sus constantes.
u(t) = Kpe(t) + Ki Z t 0 ∗e(τ )dτ + Kd d dte(t) (13)
Teniendo las bases del algoritmo PID definimos cada constante y su significado en este caso. u(t) es la acci´on a tomar por los motores en el tiempo t, Kp es la
constante proporcional del algoritmo que se multiplica por el error actual del algoritmo, el error es la resta entre el ´angulo deseado y el ´angulo actual. Ki es
la constante que se multiplica por el error acumulado durante toda la ejecuci´on del algoritmo, en este caso se acumulan grado de error, cuanto tiempo ha estado el p´endulo desequilibrado, este ayudara a llegar al ´angulo deseado con exactitud pero su respuesta tiene que ser r´apida y se debe utilizar saturaci´on, si no al estar mucho tiempo desequilibrado en un lado al volver al llegar al ´angulo deseado la parte integrativa ha acumulado tanto error y que desequilibra el p´endulo hacia el otro lado. Kd es la constante derivativa del algoritmo y se multiplica por la
tendencia del error entre dos instantes de tiempo, para nosotros no es mas si el p´endulo esta movi´endose en la direcci´on opuesta o no del ´angulo deseado. Para obtener el error se utiliza el ´angulo calculado a partir del cuaternion que nos devuelve el sensor. El punto de equilibrio del p´endulo esta desviado y no es exactamente opuesto a la gravedad por tanto el error es la resta del ´angulo real menos el de equilibrio.
6.3.1. Saturaci´on integrativa
La parte integrativa acumula el error durante la ejecuci´on del algoritmo, el error acumulado se multiplica por la constante integrativa para actuar sobre los motores. Esto nos plantea varios problemas, el error acumulado en teor´ıa solo es creciente, en nuestro caso el error acumulado puede ser tanto en una direcci´on como en otra del ´angulo deseado. Para resolver este problema utilizaremos el error con signo, el signo nos muestra en que lado esta el p´endulo sobre el pun- to de equilibrio (e.g. +5o o -5o con respecto al punto de equilibrio), esto nos
permitir´a tener un error acumulado que se acumula en ambas direcciones y al sumarse con signo puede crecer o decrecer. Otro problema que se nos plantea es el crecimiento incontrolado del error acumulado (¨Integrative Windup”), esto tiene un efecto negativo sobre el punto de equilibrio llegando a desestabilizarlo cuando el error acumulado es tan grande que no llega a reducirse a tiempo, esto se resuelve poniendo a cero el error acumulado si se da la situaci´on de que se ha cambiado de direcci´on, esto nos proporciona una reacci´on mas r´apida del termino integrativo. Por otra parte tambi´en se opta por saturar el error acumu- lado, as´ı podemos tener valores muy peque˜nos de acci´on que cambian con gran rapidez proporcionado una respuesta casi instant´anea.
6.4.
Bucle de control
El bucle de control es el responsable de obtener el estado del sistema y actuar en consecuencia, este tambi´en implementa parte de las comunicaciones cuando no se est´an haciendo c´alculos. El C´odigo 16 muestra la implementaci´on.
Codigo 16: Bucle de control v o i d d e l a y _ m s(u n s i g n e d l o n g x){
if(x> 1 0 0 0 ) { _ _ d e l a y 3 2((x/ 1 0 0 0 ) *FCY); }e l s e{ _ _ d e l a y 3 2((x*FCY) / 1 0 0 0 ) ; } } int s t a t e = ON; int x=0; int i n t e r r u p c i o n M P U = 0; int i n t r=0; f l o a t p i d _ p = 0; f l o a t p i d _ d = 0; f l o a t p i d _ i = 0; f l o a t p i d _ p i d = 0; int f i f o C o u n t=0; int f i f o B u f f e r[ 1 2 8 ] ; f l o a t kp = 0; f l o a t ki = 0; f l o a t kd = 0; f l o a t i n c l = 4 . 9 8 ; // 4 . 9 2 int aux; int var; f l o a t p i d _ i n; int s t r e n g t h 1=0; int s t r e n g t h 2=0; f l o a t err=0; f l o a t sat = 0 . 0 4 ; // 0 . 3 8 ; f l o a t e r r _ s=0; f l o a t e r r _ a n t=0; f l o a t e r r _ a c c=0; f l o a t m1 = 1; f l o a t m2 = 1; int p r i n t e d=t r u e; int q u a t _ d a t a[ 4 ] ; f l o a t q[ 4 ] ; f l o a t r o l l=0; c h a r d i r e c t i o n; f l o a t c o n t r o l _ p(){ r e t u r n err*kp; } f l o a t c o n t r o l _ d(){ f l o a t de = err-e r r _ a n t; r e t u r n kd*de/S A M P L I N G _ R A T E; } f l o a t c o n t r o l _ i(){ r e t u r n ki*f a b s(e r r _ a c c); } int m a i n (v o i d) { p r i n t S t r(" I n i t i a l i z i n g . . . \ n "); U A R T i n i t i a l i z e(); E n a b l e _ i 2 c(); d e l a y _ m s( 1 0 0 ) ; S e t u p M P U _ i 2 c(); P W M 1 i n i t i a l i z e();
P W M 2 i n i t i a l i z e(); i n i t M o t o r(); s e t T i m e r 1(A C T U A L _ P T P E R); s e t D u t y C y c l e 1(A C T U A L _ P T P E R); s e t T i m e r 2(A C T U A L _ P T P E R); s e t D u t y C y c l e 2(A C T U A L _ P T P E R); p i d _ i n = 0 . 0 5 ; kp = 1 7 5 0 ; // 1 7 5 0 ; / / 4 1 0 0 ; / / 1 1 0 0 ; / / 1 6 0 0 ; / / 8 2 5 kd = 35; // 78; / / 8 0 0 ; / / 2 ; / / 4 0 0 0 0 ki = 5 0 0 0 0 ; // 6 3 5 0 ; / / 1 1 0 0 ; / / 1 2 0 0 ; / / 5 0 0 0 r e s e t F I F O(); s e t D M P E n a b l e d(t r u e); int d m p P a c k e t S i z e = 48; w h i l e(1 ) { w h i l e (!i n t e r r u p c i o n M P U && f i f o C o u n t < 4 8 ) { e r r _ a n t = err; e r r _ s = i n c l - r o l l; err = f a b s(i n c l - r o l l); if(roll>=i n c l){ if(d i r e c t i o n==’ f ’) e r r _ a c c=0; d i r e c t i o n=’ b ’; }e l s e if(roll<i n c l){ if(d i r e c t i o n==’ b ’) e r r _ a c c=0; d i r e c t i o n=’ f ’; } e r r _ a c c += e r r _ s*S A M P L I N G _ R A T E;
if(err_acc>sat) e r r _ a c c = sat; e l s e if(err_acc< -sat) e r r _ a c c = -sat;
p i d _ p = c o n t r o l _ p(); p i d _ d = c o n t r o l _ d(); p i d _ i = c o n t r o l _ i(); p i d _ p i d = p i d _ p + p i d _ d + p i d _ i; p i d _ p i d = max(0 ,p i d _ p i d); p i d _ p i d = min(A C T U A L _ P T P E R*2 ,p i d _ p i d); x = (int)p i d _ p i d; s t r e n g t h 1 = (int) (A C T U A L _ P T P E R*2) -x*m1; s t r e n g t h 2 = (int) (A C T U A L _ P T P E R*2) -x*m2; m o t o r(d i r e c t i o n,s t r e n g t h 1,s t r e n g t h 2); e n a b l e U A R T I n t(); w h i l e(!i n t e r r u p c i o n M P U){ if(p r i n t e d==f a l s e){ if(var= = 1 ) { // p aux = (int) p i d _ p/ 1 0 ; }e l s e if(var= = 2 ) { // i aux = (int) p i d _ i/ 1 0 ; }e l s e if(var= = 3 ) { // d aux = (int) p i d _ d/ 1 0 ; }e l s e if(var= = 4 ) { // a aux = (int) p i d _ p i d/ 1 0 ; }e l s e if(var= = 5 ) { // e aux = c e i l(err* 1 0 0 ) ; }e l s e if(var= = 6 ) { // es aux = c e i l(e r r _ s* 1 0 0 ) ; }e l s e if(var= = 7 ) { // ea aux = c e i l(e r r _ a c c* 1 0 0 0 ) ; } if(var> 0 ) { p r i n t I n t(aux); p r i n t S t r(" \ n "); p r i n t e d=t r u e;
} } } p r i n t e d=f a l s e; w h i l e(s t a t e == OFF){ if (s t a t e == OFF){ m o t o r(’ b ’,(A C T U A L _ P T P E R*2) ,(A C T U A L _ P T P E R* 2 ) ) ; } } d i s a b l e U A R T I n t(); } i n t e r r u p c i o n M P U = 0; i n t r = g e t I n t S t a t u s(); f i f o C o u n t = g e t F I F O C o u n t(); if((i n t r & 0x10 ) == 16 || f i f o C o u n t >= 1 0 2 4 ) { p r i n t S t r(" O V E R F L O W !\ n "); r e s e t F I F O(); }e l s e{ if((i n t r & 0x01) == 1){ w h i l e(f i f o C o u n t<d m p P a c k e t S i z e){ f i f o C o u n t = g e t F I F O C o u n t(); } g e t F I F O B y t e s(f i f o B u f f e r, d m p P a c k e t S i z e); f i f o C o u n t -= d m p P a c k e t S i z e; d m p G e t Q u a t e r n i o n(q u a t _ d a t a, f i f o B u f f e r); q[0] = q u a t _ d a t a[0] / 1 6 3 8 4 . 0 ; q[1] = q u a t _ d a t a[1] / 1 6 3 8 4 . 0 ; q[2] = q u a t _ d a t a[2] / 1 6 3 8 4 . 0 ; q[3] = q u a t _ d a t a[3] / 1 6 3 8 4 . 0 ; r o l l = a t a n 2( ( 2 * (q[ 0 ] *q[1] + q[ 2 ] *q[3])) , (1 -2*(pow(q[1] ,2)+pow(q[ 2 ] , 2 ) ) ) ) ; r o l l = r o l l * ( 1 8 0 / 3 . 1 4 1 5 ) ; } } } r e t u r n 0; }
Lo primero que se implementa es una funci´on de retardo, esta se basa en otra funci´on de retardo que se basa en ciclos de reloj, si sabemos que una instrucci´on son dos ciclos de reloj, FCY representa esta frecuencia de ejecuci´on bas´andose en la frecuencia del oscilador, en otras palabras n instrucciones por segundo. Por que los registros son de 16 bits y para no perder precisi´on las operaciones hasta y desde mil milisegundos son diferentes. En el bucle de control se implementan las funciones de control PID por separado, se utilizan variables globales para poder ser utilizadas desde cualquier punto del programa y poder ser modificadas por el protocolo de comunicaciones que se ha implementado anteriormente. Para el control proporcional tenemos el error multiplicado por la constante proporcional, para el control derivativo debemos guardar el error en el instante anterior por ello tenemos la variable err ant para ayudarnos, cuando tenemos la diferencia del error del instante anterior dividimos por el tiempo entre las dos medidas que nos devolver´a la derivada del error y esta se multiplica por la constante derivativa, la parte integrativa es el error acumulado multiplicado por la constante integrativa. El bucle de control comienza con unas rutinas de inicializaci´on, estas nos sirven para poner todos los m´odulos en marcha, reescribir el firmware del sensor y ajustar los par´ametros PID. Por prueba y error hemos obtenido los valores de
las constantes, estos valores se guardan en las variables kp, kd y ki. Despu´es de inicializar todos los m´odulos se comienza a iterar sobre el bucle de control, este esta acotado a una frecuencia de muestreo, esta esta definida por la velocidad a la que llegan los datos del sensor y es de 50Hz lo que nos proporciona un sampling rate de 0.02 segundos. El bucle de control se divide en dos partes, la primera se compone por un bucle que hace los c´alculos necesarios cuando no hay datos disponibles, si hay datos sale de dicho bucle de calculo y recoge los datos, esto se controla con la variable interrupcionMPU. Dentro del bucle de calculo se obtiene el error con y sin signo, el error con signo sirve para saber a que direcci´on estamos cayendo, si la direcci´on ha cambiado el error acumulado pasa a ser cero, y se vuelve a acumular. Luego se comprueba que el error acumulado no supera el nivel de saturaci´on, la saturaci´on es de 0.04 grados, esto hace que la ki sea muy grande para que la acci´on se note en los motores. cuanto mas peque˜na la menos acci´on se le da a los motores, pero al aumentar la i mas r´apida sera la acci´on. Cuando tenemos calculado el error acumulado se pasa a calcular la acci´on PID a trav´es de las funciones antes descritas, saturamos tambi´en la acci´on de los motores dependiendo de lo que admita el modulo PWM, tambi´en es posible modificar a trav´es de m1 y m2 la fuerza con la que cada motor act´ua en comparaci´on con el otro, esto se implemento en un principio por que los motores iniciales (Towerpro SG90) eran diferentes en los engranajes, y eso hacia que la respuesta de ambos no fuese la misma, esto puede usarse tambi´en para implementar el giro del p´endulo en la direcci´on deseada. Cuando hemos calculado y mientras no tengamos ning´un dato disponible podemos hacer uso de las comunicaciones por lo que se activa las interrupciones UART, solo se imprime una vez por vuelta y esperamos en un bucle de espera activa a que ocurra otra interrupci´on externa, cuando ocurre se vuelven a desactivar las interrupciones UART. La segunda parte del bucle de control es la de recoger los datos por I2C cuando ocurre una
interrupci´on externa. Cuando hay una interrupci´on debemos comprobar de que tipo es leyendo cierto registro del sensor. Esto nos puede decir si se ha dado un overflow o si los datos est´an preparados, el overflow ocurre cuando el buffer de 1024 del sensor se llena. En caso de que sean que los datos est´an preparados se lee el registro FIFO 48 veces para recoger el paquete FIFO, cuando ya hemos recogido el paquete podemos utilizar desplazamiento de bits para recoger los datos de los cuaterniones, para convertirlos a un valor han de ser divididos por 16364, as´ı obtenemos un valor real, a partir de ah´ı usando atan2 convertimos uno de los ejes a ´angulo de euler y despu´es a grados con signo. Con estos datos podemos volver a calcular la acci´on de los motores volviendo al bucle de calculo hasta que se vuelva a tener nuevos datos.
7.
Conclusi´on
No todos los objetivos del proyecto has sido cumplidos, la dificultad de hacer un proyecto sin conocimiento previo de la programaci´on de microcontroladores ha sido un gran reto de auto aprendizaje. Los conocimientos adquiridos du- rante la realizaci´on del proyecto han sido extensos, desde la programaci´on de microcontroladores, nuevos conceptos de los controladores PID, comunicaciones entre m´odulos, control de motores y muchos mas. Los problemas a los que me he tenido que enfrentar vienen por mi desconocimiento del tema, aun se puede mejorar mas el p´endulo haciendo una estructura impresa en un 3D, esto obli- gar´ıa a volver a buscar nuevos par´ametros PID ya que el peso de la estructura cambiar´ıa. Tambi´en otro objetivo no cumplido es la realizaci´on de una nueva placa integrada para que integrase todos los componentes en la misma y no hacer uso de breadboards. La estructura se habr´ıa hecho usando el programa OpenSCAD para dibujar todos los componentes por separado y luego unirlos usando tornillos o otro tipo de adhesivo para pl´asticos. Una primera maqueta de la posible realizaci´on del p´endulo fue creada al principio del proyecto y es la que se muestra en la figura 26
Figura 26: Primera maqueta en OpenSCAD
Se habr´ıa utilizado el programa Orkad o Eagle para crear la placa que in- tegrase todos los componentes, por no tener experiencia en dise˜no de placas usando programas de modelado no ha sido posible la realizaci´on de dicha par- te del proyecto. Otro programa que no se menciona en el proyecto es Proteus, este programa sirve para realizar simulaciones bastante exactas del microcon- trolador, esto ha servido a modo de aprendizaje para ver como funcionan los m´odulos sin tenerlos f´ısicamente, la ´unica desventaja de este programa es que no contiene todos los modelos de microcontroladores y se debe escoger uno lo
mas parecido posible para poder hacer pruebas con el. Este proyecto a sido un gran lecci´on de aprendizaje para introducirme a mi mismo en el mundo de los microcontroladores y sistemas empotrados, algo que no se ense˜na en tanto detalle en la universidad.
Referencias
[1] Javier Graci´a Carpio. Repositorio de Grafica. url: https://github.com/ jagracar/grafica.
[2] Debra. Gyroscopes and Accelerometers on a Chip. url: http : / / www . geekmomprojects.com/gyroscopes-and-accelerometers-on-a-chip/. [3] Invensense. Ficha tecnica del sensor MPU9150. url: http : / / store .
invensense . com / datasheets / invensense / MPU - 9150 _ DataSheet _ V4 % 203.pdf.
[4] Microchip. Ficha tecnica de la familia dsPIC33F. url: http : / / www . microchip . com / pagehandler / en - us / family / 16bit / architecture / dspic33f.html?f=4&utm_source=&utm_medium=MicroSolutions&utm_ term=&utm_content=&utm_campaign=dsPIC+DSCs.
[5] Jeremiah van Oosten. Understanding Quaternions. url: http : / / www . 3dgep.com/understanding-quaternions/.
[6] Jeff Rowberg. Firmware de Jeff Rowberg. url: https : / / github . com / jrowberg/i2cdevlib/tree/master/Arduino/MPU9150.
[7] Colton Shane. The Balance Filter. url: https://b94be14129454da9cf7f056f5f8b89a9b17da0be. googledrive.com/host/0B0ZbiLZrqVa6Y2d3UjFVWDhNZms/filter.pdf.
[8] Wikipedia. Hisotria de la familia dsPIC33F. url: https://en.wikipedia. org/wiki/PIC_microcontroller.
[9] Wikipedia. PID controller. url: https://en.wikipedia.org/wiki/PID_ controller.