Implementación de un regulador PID en un dsPIC33F

Proyecto Final de Carrera Implementaci´on de un Regulador PID en un dsPIC33F Alumno: Francisco Salavert Torres Director: Pascual P´erez Blasco Ingenier´ıa T´ecnica en Inform´atica de sistemas 15 de diciembre de 2010 2 ´ Indice general 1. Introducci´on 7 1.1. Visi´ongeneral........................... 7 1.2. Motivaci´on............................. 9 1.3. Objetivos ............................. 12 1.4. Organizaci´on de los contenidos . . . . . . . . . . . . . . . . . . 13 2. Regulador PID 15 2.1. T´ermino Proporcional . . . . . . . . . . . . . . . . . . . . . . 17 2.2. T´erminoIntegral ......................... 18 2.3. T´ermino Derivativo . . . . . . . . . . . . . . . . . . . . . . . . 19 2.4. Ecuaci´on del regulador . . . . . . . . . . . . . . . . . . . . . . 19 2.5. Ecuaci´on del regulador discreta . . . . . . . . . . . . . . . . . 20 3. Motor de corriente continua 21 3.1. Modificar la velocidad de giro, PWM . . . . . . . . . . . . . . 22 3.2. Posicion y velocidad, encoders . . . . . . . . . . . . . . . . . . 23 3.2.1. Control de velocidad . . . . . . . . . . . . . . . . . . . 24 3.2.2. Control de posici´on . . . . . . . . . . . . . . . . . . . . 24 3.3. Sentido de giro, puente H . . . . . . . . . . . . . . . . . . . . 25 4. Implementaci´on 27 4.1. Microcontroladores . . . . . . . . . . . . . . . . . . . . . . . . 28 3 4´ INDICE GENERAL 4.2. dsPIC ............................... 29 4.3. Esquema del montaje y simulaci´on . . . . . . . . . . . . . . . 30 4.4. Programaci´on en C del algoritmo PID . . . . . . . . . . . . . 32 4.4.1. Regulador PID de Posici´on en C . . . . . . . . . . . . . 32 4.4.2. Regulador PID de Velocidad en C . . . . . . . . . . . . 34 4.5. Programaci´on usando las librer´ıas del dsPIC . . . . . . . . . . 36 4.5.1. Funciones PID . . . . . . . . . . . . . . . . . . . . . . 36 4.5.2. Funciones implementadas en ensamblador . . . . . . . 39 4.5.3. Ejemplo de uso de las funciones PID . . . . . . . . . . 43 4.5.4. Regulador PID de Posici´on con librerias . . . . . . . . 45 4.5.5. Regulador PID de Velocidad con librerias . . . . . . . . 46 5. Tareas, FreeRTOS 49 5.1. Implementaci´on de tareas . . . . . . . . . . . . . . . . . . . . 50 5.2. Tiempoabsoluto ......................... 52 5.3. Ejemplo de implementaci´on de una tarea . . . . . . . . . . . . 53 Ap´endices 55 A. Configuraci´on del m´odulo PWM 57 A.1. Registros de configuraci´on . . . . . . . . . . . . . . . . . . . . 57 A.1.1.P1TCON ......................... 58 A.1.2.P1TPER.......................... 58 A.1.3.PWM1CON1 ....................... 58 A.1.4. P1DTCON1 y P1DTCON2 . . . . . . . . . . . . . . . 58 A.1.5.P1DC1........................... 58 A.1.6.P1OVDCON ....................... 59 A.2.Tiempomuerto .......................... 59 A.2.1. Ejemplo de inicializaci´on . . . . . . . . . . . . . . . . . 60 ´ INDICE GENERAL 5 B. Configuraci´on de m´odulo QEI 61 B.1. Registros de configuraci´on . . . . . . . . . . . . . . . . . . . . 62 B.1.1.QEI1CON......................... 62 B.1.2.DFLT1CON........................ 63 B.1.3.POS1CNT......................... 63 B.1.4.MAX1CNT ........................ 63 B.1.5. Ejemplo de inicializaci´on . . . . . . . . . . . . . . . . . 64 C. Configuraci´on de m´odulo ADC 65 C.1.Funcionamiento.......................... 65 C.2. Registros de configuraci´on . . . . . . . . . . . . . . . . . . . . 66 C.2.1.AD1CON1......................... 66 C.2.2.AD1CON2......................... 66 C.2.3.AD1CON3......................... 66 C.2.4.AD1PCFGL........................ 67 C.2.5. AD1CHS0 y AD1CHS123 . . . . . . . . . . . . . . . . 67 C.3. Ejemplo de inicializaci´on . . . . . . . . . . . . . . . . . . . . . 68 I. main.c 71 6´ INDICE GENERAL Cap´ıtulo 1 Introducci´on Hoy en d´ıa todo es autom´atico, por ejemplo un calefactor o aire acondicionado, s´olo con elegir la temperatura, enfr´ıa o calienta una habitaci´on. Los veh´ıculos actuales son capaces de mantener una velocidad establecida por el conductor sin tener que presionar el pedal del acelerador, todo ello es posible con la ayuda de reguladores y la teor´ıa de control. 1.1. Visi´on general Los reguladores permiten respuestas muy r´apidas ante cambios en el sistema, lo cual aumenta la precisi´on de ´estos. Algunos sistemas no necesitan tal velocidad como por ejemplo el termostato de un aire acondicionado, ya que transcurren varios segundos hasta que la temperatura var´ıa, una persona seria capaz de encender o apagar el aire acondicionado a gusto, como si se tratase de un simple ventilador, que se enciende si hace calor y se apaga cuando ya no lo hace, es mucho m´as c´omodo que se encienda y se apague 7 8CAP´ ITULO 1. INTRODUCCI ´ ON autom´aticamente para no tener que estar pendiente de la temperatura. Estos sistemas no necesitan altos tiempos de respuesta sin embargo otros muchos sistemas no podr´ıan ser controlados si no es mediante reguladores, la velocidad de respuesta es cr´ıtica y ser´ıan imposibles de controlar, por ejemplo el antibloqueo de frenos en un coche ABS, que necesita ser activado en el momento preciso en que las ruedas se bloquean para evitar que el coche se deslice sobre el asfalto. La teor´ıa de control permite estudiar y tratar sistemas din´amicos. El control digital es la implementaci´on de un regulador mediante un algoritmo que se ejecuta en un computador. Este regulador proporciona las se˜nales de control necesarias para que el sistema o proceso tenga un comportamiento determinado. Esta clase de sistemas est´an gobernados por un regulador que decide como actuar. Para ello deben conocer a donde quieren llegar, por ejemplo una temperatura o una velocidad, y donde est´an, es decir, cual es la temperatura o la velocidad actual. Un sistema controlado est´a en bucle cerrado, de forma que la salida del sistema se compara con la referencia y cuando la diferencia entre la referencia y la salida es cero el sistema est´a en el estado deseado. El diagrama de bloques correspondiente a este sistema de control se muestra en la figura 1.1. 1.2. MOTIVACI ´ ON 9 ref + -eRegulador Proceso uy Computador A/D D/A Figura 1.1: Sistema controlado por un regulador discreto. La implementaci´on en un computador necesita que las se˜nales sean discretas o digitales, para ello se usan convertidores anal´ogico/digital, los cuales muestrean la se˜nal y toman valores para distintos instantes de tiempo. Estos valores discretos son procesados por el computador uno a uno, a continuaci´on a partir de los nuevos valores crea la se˜nal de control que le debe llegar al proceso y se conviertiendola en continua o anal´ogica mediante convertidores digital/anal´ogico. Esto se debe a que los computadores deben operar con valores concretos y de una precisi´on determinada. Aunque este proceso sea mas complejo que el procesamiento anal´ogico y tenga un rango de valores discretos, el procesamiento digital ofrece detecci´on y correcci´on de errores y es menos vulnerable a ruido, lo cual es una ventaja en muchas, aunque no todas, las aplicaciones. 1.2. Motivaci´on Los principales motivos que han llevado a la realizaci´on de este proyecto consisten en el inter´es por conocer mejor los sistemas empotrados y sus aplicaciones. Durante la carrera de se ha tocado este tema principalmente de 16 CAP´ ITULO 2. REGULADOR PID Mediante el ajuste de estas tres constantes en el algoritmo de control PID, el regulador es capaz de proporcionar acciones de control espec´ıficas a los requerimientos de un sistema. La respuesta del regulador se puede describir como la capacidad de respuesta ante un error, el grado en que el regulador llega m´as all´a del punto de ajuste, y la oscilaci´on del sistema. El ajuste de las constantes se logra mediante el an´alisis del sistema se puede llegar a ajustar el tiempo de respuesta, que es cuanto tiempo tarda el regulador en llevar al sistema a cierto estado. Tambi´en se puede ajustar que el sistema llegue sin errores de posici´on y minimizar las sobreoscilaciones alrededor de la referencia o estado deseado. Sin embargo esto no siempre se puede ajustar perfectamente ya que si se quiere una respuesta muy r´apida las oscilaciones son inevitables ya que el sistema no puede responder correctamente en tan poco tiempo, un ejemplo de esto es querer que un motor alcanze una determinada posici´on y que debido a la inercia se pase de la referencia y oscile por arriba y por debajo hasta llegar a la posici´on determinada por la referencia. Es claramente un efecto negativo ya que por intentar aumentar el tiempo de respuesta se ha provocado una sobreoscilaci´on y como consecuencia se ha relentizado la respuesta. Es importante mencionar que el uso de un regulador PID no garantiza un control ´optimo ni estabilidad al sistema. Hay sistemas que son inestables. Un sistema es inestable si el valor regulado oscila indefinidamente, o s´ı dicho valor diverge sin limite del estado estable. 2.1. T´ ERMINO PROPORCIONAL 17 Figura 2.1: Diagrama de bloques de un Regulador PID. Algunas aplicaciones s´olo requieren el uso de uno o dos par´ametros para proporcionar un control apropiado al sistema, para no usar un par´ametro simplemente hay que poner la ganancia del par´ametro a cero. Dependiendo de los par´ametros activos, los reguladores se pueden llamar PI, PD, P o I. Los reguladores PI son muy comunes ya que la accion derivativa D es sensible al ruido. Por otro lado la ausencia de la acci´on integral puede evitar que el sistema llegue al valor deseado. 2.1. T´ermino Proporcional El t´ermino proporcional modifica la salida proporcionalmente con el error actual. La respuesta proporcional se puede ajustar multiplicando el error por una constante Kp, conocida como ganancia proporcional. P=Kpe(t)(2.2) Donde ues la salida y eel error en un instante de tiempo, de este modo cuanto m´as grande sea el error m´as r´apida ser´a la respuesta, y conforme el 18 CAP´ ITULO 2. REGULADOR PID error se reduzca, la respuesta ser´a m´as peque˜na, hasta que finalmente el error sea cero y la respuesta nula ya que el valor deseado es el mismo que el valor actual. Un controlador proporcional no siempre alcanzar´a su valor objetivo, conservando un error de estado estacionario. Para eliminar este error hay que usar el t´ermino integral. 2.2. T´ermino Integral El t´ermino integral es proporcional a la magnitud del error y a la duraci´on del error. Es decir, la suma de todos los errores en cada instante de tiempo o como su nombre indica la integraci´on de los errores. ´ Esta suma compensa la diferencia que deber´ıa haber sido corregida anteriormente. El error acumulado se multiplica por la ganancia integral Kique indicar´a la cantidad de acci´on integral respecto de toda la acci´on de control. I=KiZt 0 e(τ)dτ (2.3) Al usar el integral junto al proporcional se acelera el movimiento del sistema llegando antes al valor deseado, es decir, menor tiempo de respuesta. Adem´as elimina el error estacionario que se produce al usar un regulador proporcional. Sin embargo el t´ermino integral tiene en cuenta los errores acumulados en el pasado y puede hacer que el valor actual sobrepase el valor deseado lo cual crear´a un error en el otro sentido, creando oscilaciones alrededor del valor deseado. 2.3. T´ ERMINO DERIVATIVO 19 2.3. T´ermino Derivativo El t´ermino derivativo calcula la variaci´on del error mediante la pendiente del error en cada instante de tiempo, es decir, la primera derivada con respecto al tiempo, y multiplica esa variaci´on del error con la ganancia derivativa Kd, la cual indica la cantidad de acci´on derivativa respecto de toda la acci´on de control. D=Kd d dte(t)(2.4) El derivativo ralentiza la tasa de cambio de la salida del regulador y cuanto m´as cerca esta del valor deseado, se hace a´un mas lento. Por tanto el uso del derivativo sirve para disminuir las oscilaciones producidas por el control integral y as´ı mejorar la estabilidad del proceso de regulaci´on. Uno de los inconvenientes es que al hacer la derivada, el ruido es amplificado, y hace al regulador m´as sensible a las interferencias llevando al sistema a la inestabilidad en el peor de lo casos. 2.4. Ecuaci´on del regulador Los t´erminos proporcional, integral y derivativo son sumandos para hallar la salida del regulador PID, sabiendo que u(t) es tal salida, la ecuaci´on que define al algoritmo es la siguiente: u(t)=Kpe(t)+KiZt 0 e(τ)dτ +Kd d dte(t)(2.5) Donde hay que tener en cuenta las siguientes consecuencias: La ganancia proporcional Kp. Cuanto mayor es, m´as r´apida es la respuesta ya que 20 CAP´ ITULO 2. REGULADOR PID cuanto m´as grande es el error, m´as grande es la acci´on proporcional. Valores muy grandes pueden ocasionar oscilaciones o incluso la inestabilidad. La ganancia integral Ki. Valores grandes eliminan m´as r´apidamente los errores estacionarios. Sin embargo puede provocar mayor sobreoscilaci´on. La ganancia derivativa Kd. Cuanto m´as grande es, m´as se reduce la sobreoscilaci´on, pero ralentizan el tiempo de respuesta, y pueden llevar al sistema a la inestabilidad debido a la amplificaci´on del ruido en el calculo diferencial del error. 2.5. Ecuaci´on del regulador discreta Un computador solo es capaz de procesar valores discretos y finitos, as´ı que para poder implementar el algoritmo de control de un regulador PID, es necesario discretizar la ecuaci´on del regulador PID, una vez discretizada se calcula para cada uno de los instantes de tiempo muestreados el valor de la acci´on de control. Para cada instante de tiempo se vuelve a calcular una nueva acci´on de control utilizando la nueva salida del sistema, esto permitir´a al sistema avanzar hacia el estado deseado marcado por la referencia con cada nuevo c´alculo. La ecuaci´on discreta del regulador para poder ser implementada en un computador se puede expresar mediante la siguiente ecuaci´on. u(t)=Kpe(t)+KiTs t X k=0 ek+Kd e(t)−e(t−1) Ts (2.6) Donde e(t)es el error de la respuesta del sistema en el instante t,Tes periodo de muestreo de la se˜nal y Kp,Ki, y Kdson la ganancia proporcional, integral y derivativa del regulador, respectivamente. Cap´ıtulo 3 Motor de corriente continua El uso de motores de CC (corriente continua) est´a muy extendido, hoy en d´ıa se pueden encontrar en cualquier dispositivo electromec´anico. Controlar la posici´on y la velocidad de estos motores es muy sencillo convirti´endose en una buena opci´on a la hora de crear aplicaciones de automatizaci´on y control. Los motores de corriente continua simplemente necesitan ser conectados a una fuente de corriente continua o una bater´ıa compatible para que se pongan en marcha. Es posible modificar la velocidad bajando o subiendo la alimentaci´on, girando m´as lento o m´as r´apido seg´un el voltaje. Al cambiar la polaridad el motor gira en sentido contrario y si lo desconectas, por inercia sigue girando hasta que se para. 21 22 CAP´ ITULO 3. MOTOR DE CORRIENTE CONTINUA 3.1. Modificar la velocidad de giro, PWM Para regular la velocidad de un motor de continua se usa la modulaci´on por ancho de pulsos (pulse-width modulation PWM). El PWM consiste en modificar la cantidad de energ´ıa que recibe un motor. Si se conecta un motor a una fuente directamente el motor comienza a acelerar hasta que alcanza cierta velocidad, si en ese momento se desconecta, el motor decelera hasta que se para. As´ı pues al conectar y desconectar repetidamente el motor de la fuente en intervalos peque˜nos el motor no llega a pararse y gira mas despacio. De este modo dependiendo del tiempo que se mantenga conectado y desconectado se pueden obtener distintas velocidades, esto se conoce como ciclo de trabajo D. D=τ T(3.1) Donde τrepresenta el tiempo que est´a conectado y Tel periodo. Por ejemplo, con un periodo de un segundo, se deja conectado 0.7 segundos, se dice que el ciclo de trabajo es del 70 %. Si el ciclo es 0 % se considera apagado y si es 100 % se considera siempre conectado. Conectado Ciclo Desconectado Ciclo de trabajo 70% Figura 3.1: Ciclo de trabajo. 3.2. POSICION Y VELOCIDAD, ENCODERS 23 Ya que el sistema que se pretende controlar es un motor de continua, se puede realizar tanto un control de posici´on como un control de velocidad. El control de posici´on mueve el motor una distancia determinada para despu´es detenerse mientras que el control de velocidad establece una velocidad a la cual debe moverse. Con esta t´ecnica generando una se˜nal con forma de pulso peri´odico, se puede regular la velocidad y la posici´on de un motor de corriente continua. 3.2. Posicion y velocidad, encoders Para medir la velocidad, posici´on o aceleraci´on de estos motores se suele usar un encoder digital, un encoder digital consta de un disco codificado con ranuras y enganchado al eje de tal manera que el disco gira con el eje. Un sensor detecta el paso de las ranuras y de esta forma sabiendo cuantas ranuras tiene el disco se puede deducir cuantas ranuras son una vuelta, por tanto conociendo el numero de vueltas por unidad de tiempo se puede deducir la velocidad o simplemente cuanto se ha movido. Figura 3.2: Disco encoder incremental. 24 CAP´ ITULO 3. MOTOR DE CORRIENTE CONTINUA Normalmente el eje del motor no est´a conectado directamente al sistema, por ejemplo una rueda, suele utilizar una reductora. Una reductora es un mecanismo compuesto por un conjunto de engranajes que modifica la velocidad del motor, adem´as el eje del motor no debe soportar el peso del sistema y es la reductora junto con un sistema de transmisi´on el que soporta toda la carga. De este modo conociendo la relaci´on de la reductora y contando las ranuras que se detectan por unidad de tiempo se puede deducir la posici´on o la velocidad. 3.2.1. Control de velocidad El control de velocidad implica obtener el numero de pulsos detectados por el paso de las ranuras en cada instante de tiempo. Es decir, pulsos por unidad de tiempo, el valor obtenido ser´a la salida del sistema, en concreto la velocidad actual del motor, que se restar´a a la referencia para obtener la siguiente acci´on de control. As´ı que es importante conocer el tiempo entre acciones de control ya que este periodo servir´a para conocer la velocidad del motor entre instantes de tiempo. 3.2.2. Control de posici´on El control de posici´on mira los pulsos detectados desde que el sistema se puso en funcionamiento, y los resta con la referencia a medida que avance el tiempo y se ejecuten acciones de control, el error se ir´a reduciendo pues la cuenta total de pulsos se ir´a acercando al valor de posici´on establecido por la referencia conforme gire el motor, y cuando la resta sea cero, el motor se parar´a indicando que ha llegado a la posici´on deseada. 3.3. SENTIDO DE GIRO, PUENTE H 25 3.3. Sentido de giro, puente H El sentido de giro de un motor de corriente continua est´a relacionado con la polaridad de la corriente que se aplica en los bornes. Un puente H consiste de cuatro transistores y un motor en medio, este dise˜no se asimila a la letra H y de ah´ı su nombre. Entrada 1 Entrada 2 Vcc T1 T2 T3 T4 M Figura 3.3: Esquema de un puente H. Al aplicar un cero l´ogico en la entrada 1 y un uno l´ogico en la entrada 2, la corriente ir´a desde el transistor T1 hacia el T4, lo cual polarizar´a el motor haci´endolo girar en un sentido, si cambiamos las entradas, un uno l´ogico en la entrada 1 y un cero l´ogico en la entrada 2, la corriente circular´a desde el transistor T2 hacia el T3 polarizando el motor tal que girar´a en sentido contrario. Si se aplica cero l´ogico en ambas entradas el motor no estar´a polarizado quedando liberado, esto quiere decir que se puede hacer girar con la mano en ambos sentidos. Del mismo modo al poner uno l´ogico en las dos entradas se quedar´a fijo y no se podr´a girar manualmente en ning´un sentido. 32 CAP´ ITULO 4. IMPLEMENTACI ´ ON 4.4. Programaci´on en C del algoritmo PID Como se ha explicado en el cap´ıtulo 2 para implementar el algoritmo de control hay que programar la siguiente ecuaci´on: u(t)=Kpe(t)+KiTs t X k=0 ek+Kd e(t)−e(t−1) Ts (4.1) 4.4.1. Regulador PID de Posici´on en C Para programar el PID, se usar´a un sencillo ejemplo en lenguage C. El ejemplo consiste en controlar la posici´on de un motor de corriente continua mediante un regulador PID. La salida del sistema ser´a la posici´on del motor, el dsPIC calcula la posici´on a partir de los valores almacenados en un registro del m´odulo QEI, el m´odulo QEI lee los pulsos proporcionados por el encoder del motor e incrementa este registro con cada pulso, los pulsos indican cuanto a avanzado el motor. As´ı, en cada iteraci´on se sabe cuanto ha avanzado, ´esto causar´a que el error disminuya conforme avance el motor. La salida del regulador se escribir´a en un registro del m´odulo PWM, este registro indica el ciclo de trabajo de la se˜nal PWM generada. Al disminuir el error la acci´on de control tambi´en lo har´a y por lo tanto en cada iteraci´on la se˜nal PWM disminuir´a hasta que la salida del regulador sea cero y por tanto, el registro contendr´a un cero, lo cual significar´a que el ciclo de trabajo es del 0 % y el motor se parar´a. 4.4. PROGRAMACI ´ ON EN C DEL ALGORITMO PID 33 float ref;// referencia float y;// salida del sistema float e;// error float ek =0; // suma de todos los errores float e_1 =0; // error del instante de tiempo anterior float Ts =0.1; // periodo de muestreo de la senyal float u;// salida del controlador float Kp =3; // ganancia proporcional float Ki =0.1; //ganancia integral float Kd =1; // ganancia derivativa while (1) { // posicion actual y = POS1CNT;/* Registro donde se encuentra la salida del sistema , es decir el avance total del motor */ // regulador PID e = ref -y;// error actual ek += e; // acumulo todos los errores para el integral u= Kp*e+Ki *Ts *ek +kd *(e - e_1 )/ Ts; // accion de control P1DC1 =u; // Registro donde se escribe el ciclo de trabajo e_1 =e;// error para el instante anterior e(t -1) } En el ejemplo se ha usado un bucle while, calculando la acci´on de control tan r´apido como puede la m´aquina, sin embargo la acci´on de control debe ejecutarse cada cierto periodo seg´un el sistema que se est´e regulando, por ello cada acci´on de control ha de ejecutarse activando alguna interrupci´on, utilizando tareas, o simplemente colocando una espera en el bucle, el tiempo entre cada ejecuci´on de las acciones de control marcar´a el periodo. 34 CAP´ ITULO 4. IMPLEMENTACI ´ ON 4.4.2. Regulador PID de Velocidad en C Con el mismo montaje que el ejemplo anterior se pretende controlar la velocidad de un motor de corriente continua mediante un regulador PID. La principal diferencia frente al control de posici´on es que se debe calcular el n´umero de pulsos detectados en cada ciclo del bucle, esta cantidad de pulsos dividida por el tiempo que tarda en ejecutarse un ciclo, permite obtener la velocidad del motor que es la salida del sistema. En el momento que alcance la velocidad deseada, el error ser´a cero, esto provocar´a que la se˜nal PWM sea cero y el motor se pare durante en esa acci´on de control provocando nuevamente un error que intentar´a regular la acci´on de control del instante de tiempo siguiente. As´ı pues el motor alcanzar´a y mantendr´a la velocidad establecida por la referencia. float ref ;// referencia float y;// salida del sistema float e;// error float ek =0; // suma de todos los errores float e_1 =0; // error del instante de tiempo anterior float Ts =0.1; // periodo de muestreo de la senyal float u;// salida del controlador float Kp =3; // ganancia proporcional float Ki =0.1; //ganancia integral float Kd =1; // ganancia derivativa float ultimoQEItick ; float cicloQEIticks ; float actualQEItick ; ultimoQEItick = POS1CNT ; // se almacena el ultimo tick 4.4. PROGRAMACI ´ ON EN C DEL ALGORITMO PID 35 while (1) { /* velocidad = cicloQEIticks / tiempo de ciclo */ actualQEItick = POS1CNT ; /* Guardar la cuenta de pulsos QEI */ cicloQEIticks = actualQEItick - ultimoQEItick ; /* Restar con el instante anterior */ ultimoQEItick = actualQEItick ; /* Actualizar la cuenta de pulsos anterior */ // Velocidad actual y = cicloQEIticks ;/* Registro donde se encuentra la salida del sistema , es decir la velocidad del motor en este instante de tiempo */ // regulador PID e = ref -y;// error actual ek += e; // acumulo todos los errores para el integral u= Kp*e+Ki *Ts *ek +kd *(e - e_1 )/ Ts; // accion de control P1DC1 =u; // Registro donde se escribe el ciclo de trabajo e_1 =e;// error para el instante anterior e(t -1) } 36 CAP´ ITULO 4. IMPLEMENTACI ´ ON 4.5. Programaci´on usando las librer´ıas del dsPIC En el compilador C30 de microchip se incluyen distintas librer´ıas las cuales incluyen: librer´ıa C est´andar, librer´ıa DSP y librer´ıa de funciones matem´aticas, tanto en como fija como coma flotante. La librer´ıa para DSP proporciona un conjunto de funciones r´apidas y optimizadas para aplicaciones de procesamiento digital de se˜nales, incrementa significativamente el rendimiento frente a las equivalentes en C. Se puede utilizar con cualquier dispositivo dsPIC. Est´a escrita principalmente en lenguaje ensamblador y hace un uso extensivo del juego de instrucciones y recursos hardware del dsPIC. Proporciona funciones para operar con vectores, matrices, filtros, transformadas y manejo de ventanas. En concreto se usar´an las funciones que permiten calcular la acci´on de control de un regulador PID en vez de implementarlo en C directamente como se ha hecho en el apartado anterior. 4.5.1. Funciones PID Para el c´alculo del PID lo primero es declarar una estructura tPID. La estructura contiene distintas variables que almacenar´an los datos usados por las funciones de c´alculo PID encargadas recibir la referencia y obtener la acci´on de control. 4.5. PROGRAMACI ´ ON USANDO LAS LIBRER´ IAS DEL DSPIC 37 typedef struct { fractional * abcCoefficients ; /* Puntero a los coeficientes que se encuentran en el espacio de memoria X, estos coeficientes se deriva de los valores de las ganancias PID: Kp , Ki y Kd */ fractional * controlHistory ; /* Puntero a las 3 ultimas muestras alamcenadas en el espacio de memoria Y, la primera es la mas reciente */ fractional controlOutput ; /* Salida del regulador PID */ fractional measuredOutput; /* Valor medido de la respuesta del sistema */ fractional controlReference ; /* Referencia del sistema */ } tPID ; 38 CAP´ ITULO 4. IMPLEMENTACI ´ ON A los espacios de memoria se acceden mediante diferentes unidades de direccionamiento y distintas rutas de datos. Las instrucciones DSP con dos operandos como las instrucciones MAC, acceden a los espacios de memoria X e Y de forma separada para poder hacer lecturas simult´aneas de ambos operandos y as´ı poder completarse en un ciclo de reloj. Las funciones encargadas del c´alculo de la acci´on de control est´an implementadas en ensamblador lo cual agiliza el tiempo de c´alculo y permite controlar sistemas que requieren una respuesta muy r´apida. En el fichero pid.s se encuentra la implementaci´on de dichas funciones. Este fichero se proporciona con el compilador C30. extern void PIDCoeffCalc ( fractional * kCoeffs , tPID * controller ); /* Deriva los coeficientes ABC usando las ganacias del PID : Kp , Ki y Kd. Necesita un array que contenga los valores Kp , Ki y Kd en secuencia y el puntero a la estructura de datos tPID */ extern void PIDInit ( tPID * controller ); /* Limpia las variables de estado del PID y la salida de las muestras . Necesita el puntero a la estructura de datos tPID */ extern fractional * PID ( tPID * controller ); /* Calcula la accion de control PID. Necesita el puntero a la estructura de datos tPID */ 4.5. PROGRAMACI ´ ON USANDO LAS LIBRER´ IAS DEL DSPIC 39 4.5.2. Funciones implementadas en ensamblador Las siguientes funciones se encuentran implementadas en ensamblador, y son proporcionadas por el compilador C30 en concreto en el fichero pid.s, dichas funciones son llamadas desde el programa en C. En los siguientes apartados hay ejemplos de su uso. .nolist . include " dspcommon . inc" ; fractsetup .list .equ offsetabcCoefficients , 0 .equ offsetcontrolHistory , 2 .equ offsetcontrolOutput , 4 .equ offsetmeasuredOutput , 6 .equ offsetcontrolReference , 8 . section . libdsp , code 4.5.2.1. PID ; _PID: ; tPID PID ( tPID * fooPIDStruct ) . global _PID ; provide global scope to routine _PID: ; Save working registers . push w8 push w10 push CORCON ; Prepare CORCON for fractional computation . fractsetup w8 mov [w0 + # offsetabcCoefficients ], w8 ; w8 = Base Address of _abcCoefficients array [( Kp +Ki+ Kd ), -( Kp +2 Kd ) , Kd] mov [w0 + # offsetcontrolHistory ], w10 ; w10 = Address of _ControlHistory array ( state / delay line ) 40 CAP´ ITULO 4. IMPLEMENTACI ´ ON mov [w0 + #offsetcontrolOutput], w1 mov [w0 + # offsetmeasuredOutput ], w2 mov [w0 + # offsetcontrolReference ], w3 ; Calculate most recent error with saturation , no limit checking required lac w3 , a ; A = tPID. controlReference lac w2 , b ; B = tPID. MeasuredOutput sub a ; A = tPID. controlReference - tPID . measuredOutput sac.r a, [ w10 ] ; tPID . ControlHistory [n] = Sat( Rnd (A)) ; Calculate PID Control Output clr a, [w8 ]+=2 , w4 , [ w10 ]+=2 , w5 ; w4 = (Kp+Ki+Kd),w5= _ControlHistory [n] lac w1 , a ; A = ControlOutput [n -1] mac w4*w5 ,a ,[w8 ]+=2 ,w4 ,[ w10 ]+=2 , w5 ; A += (Kp+Ki+ Kd)* _ControlHistory [n] ; w4 = -( Kp +2 Kd ) ,w5= _ControlHistory [n -1] mac w4*w5 , a, [w8], w4 ,[ w10] -=2 , w5; A += -(Kp+2Kd) * _ControlHistory [n -1] ; w4 = Kd , w5 = _ControlHistory [n -2] mac w4*w5 , a , [ w10 ]+=2 , w5 ; A += Kd * _ControlHistory [n -2] ; w5 = _ControlHistory [n -1] ; w10 = & _ControlHistory [n -2] sac.r a, w1 ; ControlOutput [n] = Sat( Rnd (A)) mov w1, [w0 + #offsetcontrolOutput] ; Update the error history on the delay line mov w5 , [ w10 ] ; _ControlHistory [n -2] = _ControlHistory [n -1] mov [w10 + #-4], w5 ; _ControlHistory [n -1] = ControlHistory [n] mov w5 , [-- w10 ] pop CORCON ; restore CORCON . pop w10 ; Restore working registers . pop w8 return 4.5. PROGRAMACI ´ ON USANDO LAS LIBRER´ IAS DEL DSPIC 41 4.5.2.2. PIDInit ; _PIDInit : ; void PIDInit ( tPID * fooPIDStruct ) . global _PIDInit ; provide global scope to routine _PIDInit : push w0 add # offsetcontrolOutput , w0 ; Set up pointer for controlOutput clr [w0] ; Clear controlOutput pop w0 push w0 ;Set up pointer to the base of ; controlHistory variables within tPID mov [w0 + # offsetcontrolHistory ], w0 ; Clear controlHistory variables ; within tPID clr [w0 ++] ; ControlHistory [n ]=0 clr [w0 ++] ; ControlHistory [n -1] = 0 clr [w0] ; ControlHistory [n -2] = 0 pop w0 ; Restore pointer to base of tPID return 48 CAP´ ITULO 4. IMPLEMENTACI ´ ON Cap´ıtulo 5 Tareas, FreeRTOS Un sistema empotrado, normalmente necesita implementar distintas tareas para que se ejecuten paralelamente, es por eso que usando un mini Kernel de tiempo real ayuda en la creaci´on y gesti´on de ´estas. FreeRTOS ofrece todas estas soluciones adem´as de incluir c´odigo de ejemplo para distintos modelos y marcas de microcontroladores. Est´a distribuido bajo la licencia GPL, aunque se pueden adquirir otras versiones como OpenRTOS que incluye mejoras profesionales y SafeRTOS que est´a certificado. Estas otras versiones no son gratuitas. Es simple y peque˜no, muy ´util para principiantes y aficionados. El planificador se puede configurar para que las tareas se ejecuten conjuntamente o para que se ejecute solo una dependiendo de la prioridad. 49 50 CAP´ ITULO 5. TAREAS, FREERTOS 5.1. Implementaci´on de tareas Para implementar una tarea en FreeRTOS lo primero es establecer una priorirad, esto se hace mediante un #define /* Prioridades de las tareas . */ # define mainBLOCK_Q_PRIORITY ( tskIDLE_PRIORITY + 2 ) # define mainCHECK_TASK_PRIORITY ( tskIDLE_PRIORITY + 3 ) #define mainCOM_TEST_PRIORITY ( 2 ) A continuaci´on hay que establecer el periodo: /* Periodos de las tareas . */ # define mainCHECK_TASK_PERIOD ( ( portTickType ) 3000 / portTICK_RATE_MS ) # define mainREFERENCIA_TASK_PERIOD ( ( portTickType ) 300 / portTICK_RATE_MS ) # define mainCONTROL_TASK_PERIOD ( ( portTickType ) 50 / portTICK_RATE_MS ) Las cabeceras de las funciones: static void vReferenciaTask ( void * pvParameter ); static void vControlVelTask ( void * pvParameter ); void inicializarQEI(void); void inicializarPWM(void); int refpos =256; 5.1. IMPLEMENTACI ´ ON DE TAREAS 51 Y las variables globales, que usar´an distintas tareas para comunicarse. En la funci´on main se llamar´a a las funciones de inicializaci´on, y se crear´an las tareas, por ´ultimo solo hay que llamar al planificador. int main ( void ) { /* Desde aqui se llama a las funciones de incializacion . */ prvSetupHardware(); xTaskCreate ( vReferenciaTask , ( signed char * ) " Referencia", configMINIMAL_STACK_SIZE , NULL, mainCHECK_TASK_PRIORITY , NULL ); xTaskCreate ( vControlVelTask , ( signed char * ) " ControlVel", configMINIMAL_STACK_SIZE , NULL, mainCHECK_TASK_PRIORITY + 1, NULL ); /* Finally start the scheduler . */ vTaskStartScheduler(); /* Will only reach here if there is insufficient heap available to start the scheduler . */ return 0; } static void prvSetupHardware ( void ) {/* Inicializacion de los modulos del dsPIC */ inicializarQEI(); inicializarPWM(); } 52 CAP´ ITULO 5. TAREAS, FREERTOS 5.2. Tiempo absoluto Para que las tareas se ejecuten en tiempos absolutos, hay que usar las siguientes instrucciones dentro del cuerpo de la funci´on que implementa la tarea, de este modo la tarea se ejecutar´a seg´un el periodo establecido. static void vMiTareaTask ( void * pvParameter ) { /* Se usa para despertar la tarea a una frecuencia determinada */ portTickType xLastExecutionTime ; /* Inicializar xLastExecutionTime la primera vez que se llama a vTaskDelayUntil (). */ xLastExecutionTime = xTaskGetTickCount(); while (1) { /* Esperar a que llegue la hora del siguiente ciclo . */ vTaskDelayUntil ( & xLastExecutionTime , mainCONTROL_TASK_PERIOD ); . . . } } 5.3. EJEMPLO DE IMPLEMENTACI ´ ON DE UNA TAREA 53 5.3. Ejemplo de implementaci´on de una tarea La siguiente tarea implementa el control de velocidad de un motor de corriente continua, usando para ello las funciones de librer´ıa de un regulador PID. static void vControlVelTask ( void * pvParameter ) { /* Used to wake the task at the correct frequency . */ portTickType xLastExecutionTime ; int ultimoQEItick ; int cicloQEIticks ; int actualQEItick ; /* Initialise xLastExecutionTime so the first call to vTaskDelayUntil () works correctly . */ xLastExecutionTime = xTaskGetTickCount(); ultimoQEItick = POS1CNT ; /* POS1CNT - Registro donde esta la cuenta de pulsos del encoder . Cuidado porque cambia a 0 al llegar al MAX1CNT . */ fooPID . abcCoefficients = & abcCoefficient [0]; fooPID.controlHistory = &controlHistory[0]; PIDInit (& fooPID ); kCoeffs [0] = Q15 (0.99999999) ; kCoeffs [1] = Q15 (0.0) ; kCoeffs [2] = Q15 (0.0) ; PIDCoeffCalc (& kCoeffs [0] , & fooPID ); 54 CAP´ ITULO 5. TAREAS, FREERTOS while (1) { /* Wait until it is time for the next cycle . */ vTaskDelayUntil ( & xLastExecutionTime , mainCONTROL_TASK_PERIOD ); // velocidad = cycleQEIticks / ciclo actualQEItick = POS1CNT ; cicloQEIticks = ultimoQEItick - actualQEItick ;//cuenta hacia abajo ultimoQEItick = actualQEItick ; // motor 256 ranuras en una vuelta fooPID . controlReference = Q15 ( refvel ); fooPID . measuredOutput = Q15 ( cicloQEIticks /0 x3E ); PID (& fooPID ); // 15 -0 PxDC1 <15:0 >: PWM Duty Cycle 1 Value bits // putc ( fooPID . controlOutput ); /* Se escribe la accion de control en el registro que establece el PWM donde 0 x3ff es el 100 % del ciclo de trabajo */ P1DC1 = fooPID . controlOutput &0 x3FF ; // sentido =( fooPID . controlOutput &0 x200 )==0 x200 ;// para el puente h } } Ap´endices 55 Ap´endice A Configuraci´on del m´odulo PWM El MCPWM (Motor Control PWM) se encarga de generar una se˜nal con forma de pulso peri´odico. El peri´odo y el ciclo de trabajo de los pulsos son programables. Dependiendo del microcontrolador puede tener varios m´odulos MCPWM, en cualquier caso la configuraci´on que se explica s´olo usar´a uno ya que solo se va a controlar un motor. A.1. Registros de configuraci´on El microcontrolador posee distintos registros con distintos bits para controlar el modo de operaci´on del MCPWM. La informaci´on de todos los registros se encuentra en el datasheet del microcontrolador. Se explicar´an los utilizados, el resto al no modificarse quedar´an en su valor por defecto (cero) y por tanto en el modo de funcionamiento correspontiente al cero. 57 64 AP´ ENDICE B. CONFIGURACI ´ ON DE M ´ ODULO QEI B.1.5. Ejemplo de inicializaci´on // POS1CNT = 0 xFFFF ; // Se incrementa con la cuenta de pulsos del encoder QEI1CONbits . QEIM = 0 b101 ; // resolucion 2x, Reset al coincidir con MAXCNT QEI1CONbits . QEISIDL = 0; // continuar en modo idle (0) QEI1CONbits . SWPAB = 0; // Phase -A y Phase -B no intercambiados QEI1CONbits . PCDOUT = 1; // activado el Position Counter Direction Status QEI1CONbits . TQGATE = 0; // Timer gate apagado QEI1CONbits . TQCKPS = 0; // Prescaler 1:1 QEI1CONbits . POSRES = 1; // Un pulso en INDEX hace un reset QEI1CONbits . TQCS =0; // Usamos clock interno para el timer QEI1CONbits . UPDN_SRC =1; // Phase -B indica direccion MAX1CNT =23; // POS1CNT se pone a 0 cuando POS1CNT es igual que MAX1CNT DFLT1CON = 0 x00F0 ; // filtro de ruido habilitado Ap´endice C Configuraci´on de m´odulo ADC C.1. Funcionamiento La conversi´on anal´ogica-digital (ADC) consiste en transformar se˜nales anal´ogicas en se˜nales digitales, para poder procesarlas de forma m´as sencilla. Las se˜nales digitales se pueden procesar en computadores, lo cual implica la posibilida de aplicar filtros para mejorar la se˜nal o eliminar el ruido. Convertir una se˜nal anal´ogica en digital consiste en obtener un valor de la se˜nal para cada instante de tiempo, a esto sele llama muestreo, y al tiempo entre la toma de muestras, periodo de muestreo. 65 66 AP´ ENDICE C. CONFIGURACI ´ ON DE M ´ ODULO ADC C.2. Registros de configuraci´on A continuaci´on se describen los distintos registros que se han de configurar para poner en funcionamiento y ejemplo sencillo de conversi´on. El m´odulo ADC tiene dos modos de funcionamiento, 4 canales de 10 bits o un canal de 12 bits. C.2.1. AD1CON1 En el modo de 10 bits se puede seleccionar usar un canal, dos canales o cuatro canales y no todos lo pines del microcontrolador pueden usarse para cualquier canal. Tambi´en se puede seleccionar el modo de autoconversi´on. Adem´as este registro inicia o para la conversi´on modificando el bit AD1CON1bits.ADON. C.2.2. AD1CON2 En este registro se pueden seleccionar los voltages de referencia VREFH y VREFL para las conversiones A/D ya sea mediante valores externos conectados a los pines o usando los valores internos del microcontrolador AVDD y AVSS. C.2.3. AD1CON3 En este registro se configura el reloj de conversi´on anal´ogica para que conincida la velocidad de muestreo de los datos con el reloj del procesador. C.2. REGISTROS DE CONFIGURACI ´ ON 67 C.2.4. AD1PCFGL En este registro se seleccionan los pines de un puerto como entradas anal´ogicas. Cuando se configura un pin como entrada anal´ogica, tambien hay que configuralo como entrada usando TRIS. C.2.5. AD1CHS0 y AD1CHS123 El m´odulo puede escanear distintos canales o no, esto se configura en AD1CON2bits.CSCNA, si se usa el modo de no escanear se usar´a el canal cero y la entrada ser´a el pin seleccionado en AD1CHS0bits.CH0SA, a su vez el canal puede muestrear dos entradas, primero una y luego la otra, A y B seleccionando el pin en AD1CHS0bits.CH0SB . Para muestrear dos entradas hay que activar el bit ALTS en el registro AD1CON2, si no siempre muestrear´a la A. En este caso para simplificar se seleccionara un canal, el cero, y solo una entrada. 68 AP´ ENDICE C. CONFIGURACI ´ ON DE M ´ ODULO ADC C.3. Ejemplo de inicializaci´on AD1CON1bits . AD12B = 0; // 10bit ADC operation AD1CON1bits . FORM = 0; // 00 = Integer (DOUT = 0000 00 dd dddd dddd ) AD1CON1bits . SSRC = 7; // 111 = Internal counter ends sampling and starts conversion ( auto - convert ) AD1CON1bits . ASAM = 1; // 1 = Sampling begins immediately after last conversion AD1CON1bits . SIMSAM = 0; // 0 = Samples multiple channels individually in sequence AD1CON2bits . CHPS = 0; // Converts channel CH0 AD1CON2bits . SMPI = 0; // 0000 = ADC interrupt is generated at the completion // of every sample / conversion operation AD1CON3bits . ADRC = 0; // ADC Clock is derived from Systems Clock AD1CON3bits . SAMC = 0; // Auto Sample Time = 0 * TAD AD1CON3bits . ADCS = 2; // ADC Conversion Clock // TAD = TCY * (ADCS + 1) = (1/40 M) * 3 = AD1CHS0bits . CH0SA = 0; // MUXA +ve input selection ( AIN0 ) for CH0 AD1CHS0bits . CH0NA = 0; // MUXA -ve input selection (VREF -) for CH0 AD1CHS123bits . CH123SA = 0; // MUXA +ve input selection ( AIN0 ) for CH1 AD1CHS123bits . CH123NA = 0; // MUXA -ve input selection (VREF -) for CH1 C.3. EJEMPLO DE INICIALIZACI ´ ON 69 AD1PCFGL = 0 xFFFF ; // AD1PCFGH / AD1PCFGL : Port Configuration Register AD1PCFGLbits . PCFG0 = 0; // AN0 as Analog Input IFS0bits . AD1IF = 0; // Clear the A/D interrupt flag bit IEC0bits . AD1IE = 0; // Do Not Enable A/D interrupt AD1CON1bits . ADON = 1; // Turn on the A/D converter TRISAbits . TRISA0 =1; // pin configured as input 70 AP´ ENDICE C. CONFIGURACI ´ ON DE M ´ ODULO ADC Anexo i main.c /* Standard includes . */ # include < stdio .h> # include <dsp .h> /* Scheduler includes . */ # include " FreeRTOS .h" # include " task .h" # include " queue .h" # include " croutine .h" /* Demo application includes . */ # include " BlockQ .h" #include "crflash.h" # include " blocktim .h" #include "integer.h" # include " comtest2 .h" #include "partest.h" 71 72 ANEXO I. MAIN.C # include " lcd .h" # include " timertest .h" /* Demo task priorities . */ # define mainBLOCK_Q_PRIORITY ( tskIDLE_PRIORITY + 2 ) # define mainCHECK_TASK_PRIORITY ( tskIDLE_PRIORITY +3) #define mainCOM_TEST_PRIORITY ( 2 ) /* The check task may require a bit more stack as it calls sprintf (). */ #define mainCHECK_TAKS_STACK_SIZE ( configMINIMAL_STACK_SIZE * 2 ) /* The execution period of the check task . */ # define mainCHECK_TASK_PERIOD ( ( portTickType ) 3000 / portTICK_RATE_MS ) # define mainREFERENCIA_TASK_PERIOD ( ( portTickType ) 300 / portTICK_RATE_MS ) # define mainCONTROL_TASK_PERIOD ( ( portTickType ) 50 / portTICK_RATE_MS ) /* The number of flash co - routines to create . */ #define mainNUM_FLASH_COROUTINES ( 5 ) /* Baud rate used by the comtest tasks . */ # define mainCOM_TEST_BAUD_RATE ( 19200 ) 73 /* The LED used by the comtest tasks . mainCOM_TEST_LED + 1 is also used . See the comtest .c file for more information . */ # define mainCOM_TEST_LED ( 6 ) /* The frequency at which the " fast interrupt test" interrupt will occur . */ # define mainTEST_INTERRUPT_FREQUENCY ( 20000 ) /* The number of processor clocks we expect to occur between each " fast interrupt test " interrupt . */ #define mainEXPECTED_CLOCKS_BETWEEN_INTERRUPTS ( configCPU_CLOCK_HZ / mainTEST_INTERRUPT_FREQUENCY ) /* The number of nano seconds between each processor clock . */ # define mainNS_PER_CLOCK ( ( unsigned short ) ( ( 1.0 / ( double ) configCPU_CLOCK_HZ ) * 1000000000.0 ) ) /* Dimension the buffer used to hold the value of the maximum jitter time when it is converted to a string . */ # define mainMAX_STRING_LENGTH ( 20 ) // paco # define mainLED_DELAY (( portTickType ) 1000/ portTICK_RATE_MS) // static void vUserLED1Task ( void * pvParameter ); 80 ANEXO I. MAIN.C fooPID . controlReference = Q15 ( refvel ); /* Set the Reference Input for your controller */ fooPID . measuredOutput = Q15 ( cicloQEIticks /0 x3E ); /* Typically the measuredOutput variable is a plant response */ /* measured from an A/D input or a sensor. */ /* In this example we manually set it to some value for */ /* demonstration but the user should note that this value will */ /* keep changing in a real application*/ // fooPID . controlOutput ; // Me falta escribir en P1DC1 el ciclo de trabajo . PID (& fooPID ); // 15 -0 PxDC1 <15:0 >: PWM Duty Cycle 1 Value bits // putc ( fooPID . controlOutput ); PORTB = fooPID . controlOutput &0 x3FF ; P1DC1 = fooPID . controlOutput &0 x3FF ; // P1DC1 = fooPID . controlOutput &0 x1FF ;// pascual // sentido =( fooPID . controlOutput &0 x200 )==0 x200 ;// para el puente h 81 } } static void vControlPosTask_P ( void * pvParameter ) { int pos; int error ; int u; int k =3; /* Used to wake the task at the correct frequency . */ portTickType xLastExecutionTime ; /* Initialise xLastExecutionTime so the first call to vTaskDelayUntil () works correctly . */ xLastExecutionTime = xTaskGetTickCount(); /* POS1CNT - Registro donde esta la cuenta de pulsos del encoder . Cuidado porque cambia a 0 al llegar al MAX1CNT . */ while (1) { /* Wait until it is time for the next cycle . */ vTaskDelayUntil ( & xLastExecutionTime , mainCONTROL_TASK_PERIOD ); // posicion pos = POS1CNT ; // porque cuenta hacia abajo . PORTB = pos ; // regulador P error = refpos - pos; if( error <0) error =0; 82 ANEXO I. MAIN.C u=k* error ; // 15 -0 PxDC1 <15:0 >: PWM Duty Cycle 1 Value bits P1DC1 =u &0 x3FF ; // P1DC1 =u &0 x3FF ; // PORTB =u &0 x3FF ; } } static void vControlPosTask_PI ( void * pvParameter ) { float pos ; float e;// error float ek =0; // suma de todos los errores float e_1 =0; // error del instante de tiempo anterior float Ts =0.1; // periodo de muestreo de la senyal float u;// salida del controlador float Kp =3; // ganancia proporcional float Ki =0.1; //ganancia integral float Kd =1; // ganancia derivativa /* Used to wake the task at the correct frequency . */ portTickType xLastExecutionTime ; /* Initialise xLastExecutionTime so the first call to vTaskDelayUntil () works correctly . */ xLastExecutionTime = xTaskGetTickCount(); /* POS1CNT - Registro donde esta la cuenta de pulsos del encoder . Cuidado porque cambia a 0 al llegar al MAX1CNT . */ 83 while (1) { /* Wait until it is time for the next cycle . */ vTaskDelayUntil ( & xLastExecutionTime , mainCONTROL_TASK_PERIOD ); // posicion pos = POS1CNT ; // porque cuenta hacia abajo . PORTB = ek; // regulador PI e = refpos -pos;// error actual if(e <0) e=0; ek += e; // acumulo todos los errores para el integral u=Kp*e+Ki*Ts*ek; // e_1 =e ;// me guardo el error para e(t -1) // 15 -0 PxDC1 <15:0 >: PWM Duty Cycle 1 Value bits P1DC1 =u; // P1DC1 =u &0 x3FF ; // PORTB =u &0 x3FF ; } } /* ----------------------------------------------------------- */ /* static void vUserLED1Task ( void * pvParameter ) { while (1) { 84 ANEXO I. MAIN.C PORTCbits . RC1 = 1; // let it shine vTaskDelay ( mainLED_DELAY ); PORTCbits .RC1 = 0; // take a break vTaskDelay ( mainLED_DELAY ); } } /*----------------------------------------------------------- */ /* static void vUserLED2Task ( void * pvParameter ) { while (1) { PORTCbits . RC2 = 1; // let it shine vTaskDelay ( mainLED_DELAY / 2); PORTCbits .RC2 = 0; // take a break vTaskDelay ( mainLED_DELAY / 2); } } // paco /*----------------------------------------------------------- */ static void prvSetupHardware ( void ) { vParTestInitialise (); // paco 85 inicializarQEI(); inicializarPWM(); /* TRISCbits . TRISC1 = 0;// OUTPUT PORTCbits .RC1 = 0;// ALL OFF 0 TRISCbits . TRISC2 = 0;// OUTPUT PORTCbits .RC2 = 0;// ALL OFF 0 */ // paco } /* ----------------------------------------------------------- */ static void vCheckTask ( void * pvParameters ) { /* Used to wake the task at the correct frequency . */ portTickType xLastExecutionTime ; /* The maximum jitter time measured by the fast interrupt test . */ extern unsigned short usMaxJitter ; /* Buffer into which the maximum jitter time is written as a string . */ static char cStringBuffer [ mainMAX_STRING_LENGTH ]; 86 ANEXO I. MAIN.C /* The message that is sent on the queue to the LCD task . The first parameter is the minimum time ( in ticks ) that the message should be left on the LCD without being overwritten . The second parameter is a pointer to the message to display itself . */ xLCDMessage xMessage = { 0, cStringBuffer }; /* Set to pdTRUE should an error be detected in any of the standard demo tasks . */ unsigned short usErrorDetected = pdFALSE ; /* Initialise xLastExecutionTime so the first call to vTaskDelayUntil () works correctly . */ xLastExecutionTime = xTaskGetTickCount(); for ( ;; ) { /* Wait until it is time for the next cycle . */ vTaskDelayUntil ( & xLastExecutionTime , mainCHECK_TASK_PERIOD ); /* Has an error been found in any of the standard demo tasks ? */ if( xAreIntegerMathsTaskStillRunning() != pdTRUE ) { usErrorDetected = pdTRUE ; sprintf ( cStringBuffer , "FAIL #1" ); 87 } if( xAreComTestTasksStillRunning () != pdTRUE ) { usErrorDetected = pdTRUE ; sprintf ( cStringBuffer , "FAIL #2" ); } if( xAreBlockTimeTestTasksStillRunning() != pdTRUE ) { usErrorDetected = pdTRUE ; sprintf ( cStringBuffer , "FAIL #3" ); } if( xAreBlockingQueuesStillRunning () != pdTRUE ) { usErrorDetected = pdTRUE ; sprintf ( cStringBuffer , "FAIL #4" ); } if( usErrorDetected == pdFALSE ) { /* No errors have been discovered , so display the maximum jitter timer discovered by the "fast interrupt test ". */ sprintf ( cStringBuffer , " %dns max jitter " , ( short ) ( usMaxJitter - mainEXPECTED_CLOCKS_BETWEEN_INTERRUPTS ) * mainNS_PER_CLOCK ); } 88 ANEXO I. MAIN.C /* Send the message to the LCD gatekeeper for display . */ xQueueSend ( xLCDQueue , & xMessage , portMAX_DELAY ); } } /* ----------------------------------------------------------- */ void vApplicationIdleHook( void ) { /* Schedule the co - routines from within the idle task hook . */ vCoRoutineSchedule (); } /* ----------------------------------------------------------- */ // paco void inicializarQEI(void) { /* The QEI module has the following four user - accessible registers . The registers are accessible either in Byte mode or Word mode. - QEIxCON : QEI Control Register - Controls QEI operation and provides status flags for the state of the module . - DFLTxCON : Digital Filter Control Register - Controls digital input filter operation . 89 - Position Count Register ( POSxCNT ) - Allows reading and writing of the 16 - bit position counter. - Maximum Count Register ( MAXxCNT ) - Holds a value that is compared to the POSxCNT counter in some operations . */ /* QEI Phase A QEA RPINR14 QEAR [4:0] QEI Phase B QEB RPINR14 QEBR [4:0] QEI Index INDX RPINR15 INDXR [4:0] */ // Seleccion de pines como entradas de la Fase A, Fase B e Indice RPINR14 =0 x1513 ; // RP19 QEA RP21 QEB RPINR15 =0 x0014 ; // RP20 INDX TRISCbits . TRISC3 = 1; TRISCbits . TRISC4 = 1; TRISCbits . TRISC5 = 1; // POS1CNT = 0 xFFFF ; QEI1CONbits . QEIM = 0 b101 ; // resolucion 4x, modo de Reset por MAXCNT QEI1CONbits . QEISIDL = 0; // continuar en modo idle (0) QEI1CONbits . SWPAB = 0; // Phase -A y Phase -B no intercambiados QEI1CONbits . PCDOUT = 1; // activado el pin de Position Counter Status