CONTROLADOR PROPORCIONAL DERIVATIVO
El controlador está implementado con el microcontrolador PIC18F4520 de Microchip, del cual se ha utilizado su gestor de interrupciones, uno de sus temporizadores, el conversor A/D y la EUSART (Enhanced Universal Synchronous Asynchronous Receiver Transmitter). En las siguientes figuras se puede visualizar la porción del esquemático correspondiente al controlador y el montaje realizado en la protoboard con fines de realizar los ensayos y ajustes necesarios antes de dar el diseño por válido.
La posición del péndulo es recogida por el conversor A/D desde un montaje realizado en base a un potenciómetro de 25KOhms que sirve tanto de eje como de sensor de inclinación. El objeto de montar de esta forma el eje es para minimizar el rozamiento propio de un potenciómetro limitando el punto de contacto a solamente el cursor que se desplaza sobre la zona resistiva y con una presión mínima.
A continuación se visualiza el esquema del conexionado de los componentes que constituyen el controlador del péndulo vertical y el detalle del montaje del potenciómetro.
Además en los extremos del carro de la impresora se han dispuesto dos sensores de final de carrera para evitar la colisión del cabezal con los extremos. Cuando el cabezal acciona uno de estos interruptores el controlador detiene el funcionamiento del motor, siendo necesario volver a posicionarlo manualmente en el centro de su recorrido y resetear el sistema.
La estructura del programa es la mostrada en la figura a continuación, en donde el flujo del programa comienza con la inicialización del microcontrolador y de las variables para luego entrar en un bucle infinito que comprende la lectura de la señal analógica del error y el acondicionado y recorte de los niveles de la misma.
Cada 1.54mS se produce una interrupción originada por el temporizador que ejecuta la rutina de control utilizando el último de los valores obtenido desde el conversor A/D.
Cada bloque de código se describe en su propia sección:
DESCRIPCIÓN DE LA RUTINA DE INICIALIZACIÓN
La inicialización comprende la configuración del microcontrolador y de los dispositivos que se van a utilizar y la asignación de los valores iniciales a las variables globales del programa. El código de esta sección se puede consultar en el listado que se presenta más abajo en esta misma página, en donde realiza primero la configuración del reloj interno de microcontrolador escogiendo un valor de 8MHz que es el mayor admitido por el dispositivo. La utilidad de una frecuencia alta es la de ejecutar toda la rutina controladora antes del siguiente ciclo de control para evitar solapamientos ya que la transmisión por el puerto serie prolonga su duración al formar parte de la rutina de control y nunca el tiempo de espera para la transmisión debe penalizar a la rutina de control.
Posteriormente se inicializa el puerto B configurándolo como salida para ser utilizado (sus 4 bits menos significativos) para enviar las señales de control del motor paso a paso al manejador. El puerto E también se configura como salida para excitar el LED de calibración a través de su bit 0. Luego le toca el turno al puerto A pero en este caso la configuración será de todas sus líneas como entradas y por último el puerto C se configurará también como entrada ya que es un requisito para utilizar la EUSART con la que comparte algunos de sus pines.
Además del puerto, la EUSART requiere configurar los registros que se utilizarán para generar la señal de transmisión de 57.600 Baudios. Una vez configurada se procede a habilitar este dispositivo.
La sección siguiente se encarga de configurar el conversor A/D seleccionando como entrada la menos significativa del puerto A (AN0) y configurando el modo en que se volcará el valor leído en los registros ADRESH:ADRESL.
Otro paso necesario es la inhabilitación del comparador ya que este comparte el puerto A con el conversor A/D y no se pueden utilizar al mismo tiempo.
Por último se inicializa el gestor de interrupciones habilitando sólo la correspondiente al temporizador que será la encargada de iniciar la ejecución cíclica de la rutina de control.
Sólo resta asignar los valores iniciales a las variables que se utilizarán dentro del programa para que el microcontrolador esté listo para ejecutar el bucle principal.
CÓDIGO DE LA RUTINA DE INICIALIZACIÓN
; Clock
bsf OSCCON,IRCF0
bsf OSCCON,IRCF1
bsf OSCCON,IRCF2 ; 8 MHz
bsf OSCCON,SCS1 ; Internal Oscilator
; Initialize PORT B: Drive the motor
clrf PORTB
clrf LATB
clrf TRISB ; all outputs
; Initialize PORT E: Calibrating LED
clrf PORTE
clrf LATE
clrf TRISE ; all outputs
; Initialize PORT A: Error, Cal, Kp & Kd
clrf PORTA
clrf LATA
setf TRISA ; all inputs
; Initialize EUSART (Port C): Telemetry
setf TRISC ; all inputs
bcf TXSTA,TX9 ; 8-bit transmission
bcf TXSTA,SYNC ; Asynchronous mode
bsf TXSTA,BRGH ; High speed
bsf BAUDCON,BRG16 ; 8-bit Baud Rate Generator
movlw .34 ; Internal counter
movwf SPBRG ; 57600 Bauds
bsf RCSTA,SPEN ; Enable EUSART
bsf TXSTA,TXEN ; Enable the transmission
; Initialize ADC
movlw b'00000000'
movwf ADCON0 ; select channel 0 (AN0)
movlw b'00001110'
movwf ADCON1 ; Only input AN0 is analog
movlw b'10000110' ; Read Right justified
movwf ADCON2 ; Clock: FOSC/64
bsf ADCON0,ADON ; converter enabled
; Initialize Comparator: Turn off
movlw 0x07
movwf CMCON ; comparator disabled
; Initialize timer0: Control cycle
; 8 bits mode
movlw b'01010011' ; Internal clock
movwf T0CON ; prescaler (divides by 16)
; Interruption
bcf RCON,IPEN ; disable priority levels
bsf INTCON,GIE ; enable all unmasked interrupts
bcf INTCON,PEIE ; disable all unmasked peripheral interrupts
bcf INTCON,TMR0IF ; clear flag timer int.
bsf INTCON,TMR0IE ; activate timer overflow interruption
; Initialize vars
movlw _SEQ_1 ; Motor sequence Literal
movwf SEQ ; Initialize Sequence
movlw _DIR_STOP ; Direction Literal
movwf DIRECTION ; Stopped
clrf ACTION ; Controler Action
movlw 0xFF
movwf DELAY ; Initial Delay
movlw 0x01
movwf COUNT ; Initial Count
clrf ENDS ; not in left end, not in right end
clrf ERR_1 ; Initial E(k-1)
clrf ERR ; Initial E(k)
;;;;;;;;;;;;;;;;;;;;;;;;; Telemetry
clrf TEL_ACT ; Control
movlw 'S'
movwf HEAD ; Head stopped
clrf ANGLE ; Initial angle
;;;;;;;;;;;;;;;;;;;;;;;;; DESCRIPCION DE LA RUTINA DE LECTURA DEL ERROR
La lectura del error o desviación del péndulo de la vertical es realizado por el conversor A/D al cual le llega a través del pin AN0 del microcontrolador la señal desde el potenciómetro ubicado sobre el cabezal de la impresora. Además se ha añadido un preset de ajuste para realizar la puesta a 0 en el modo de calibración, indicando que la posición en la que el péndulo ha sido colocado se corresponde con la que debe ser interpretada como vertical por el controlador. En la siguiente figura se puede ver el detalle del circuito de calibración.
La tensión presente en le pin AN0 es procesada por el conversor A/D y puede ser leída en los registros ADRESH:ADRESL como un número de 10 bits y ser volcada en las variables ADH y AHL.
Luego se llama a la rutina que se encargará de comprobar de que el valor se encuentra dentro del rango deseado y si es necesario realizar el recorte del valor. Debido a que se va a actuar frente a desviaciones muy pequeñas, el error se restringirá al rango ±4, valor que experimentalmente ha demostrado ser el más adecuado dando los mejores resultados.
Además dentro de esta rutina se comprobará el rango para el valor de la variable ANGLE, la que será utilizada para enviar le información telemétrica al PC indicando la inclinación del péndulo. En este caso se ha tomando un rango de valores de ±128 permitiendo enviar en 1 byte tanto el ángulo como el signo.
En la siguiente figura se observa el diagrama de la rutina que lee y procesa el error y más abajo, el listado en dónde se puede consultar el código ensamblador.
Al finalizar la rutina el error devuelto se guarda en la variable ERR para ser procesado por la rutina de control la que se ejecuta periódicamente como ya se ha mencionado anteriormente. Además si el error es nulo se enciende el LED de centrado (salida RE0 del puerto E) para indicar esta situación, dejándolo apagado en caso contrario. Por último para finalizar el bucle de lectura se introduce el retardo necesario para que el conversor A/D inicie una nueva conversión.
Nótese que esta rutina será interrumpida cada 1.54mS por la rutina de control mediante la interrupción provocada por el temporizador del microcontrolador.
CODIGO DE LA RUTINA DE LECTURA DEL ERROR
bsf T0CON,TMR0ON ; activate timer
Loop
bsf ADCON0,NOT_DONE ; Begins ADC conversion
Read
btfsc ADCON0,NOT_DONE
goto Read ; Polling for ADC value
movff ADRESH,ADH ; ADRESH -> ADH
movff ADRESL,ADL ; ADRESL -> ADL
call Calc_E ; Convert ADC value in SPEED and DIRECTION
; returns error in WREG [-4 - 4]
movwf ERR ; Updates ERR
movlw 0x02
movwf LOOP ; Counter
movlw 0x00
bcf PORTE,RE0 ; Turn off Center LED (calibrating)
cpfseq ERR
goto ADCDelay ; if ERR = 0
bsf PORTE,RE0 ; Turn on Center LED (calibrating)
ADCDelay
decfsz LOOP
goto ADCDelay ; Wait a minimum of 2 TADs before another conversion
goto Loop
;**********************************************************
; Transform Error
;**********************************************************
Calc_E
;;;;;;;;;;;;;;;;;;;;;;;;; Range test (ADH value)
movlw B'00000011' ; Mask b0 & b1
andwf ADH
movlw .3
cpfslt ADH ; if ERROR > 1536 (11 00000000)
goto OutGreater ; Out of Range (Greater)
movlw .0
cpfsgt ADH ; if ERROR < 512 (01 00000000)
goto OutLower ; Out of Range (Lower)
movlw .1
cpfseq ADH ; if ERROR is positive (0x0200 - 0x02FF)
goto Greater ; move to the right (greater)
; else move to the left (lower)
; (0x01FF - 0x0100)
Lower
;;;;;;;;;;;;;;;;;;;;;;;;; Telemetry
movff ADL,ANGLE
movlw .128
cpfsgt ANGLE ; if ANGLE < -128
movwf ANGLE ; ANGLE = -128
;;;;;;;;;;;;;;;;;;;;;;;;;
movlw .252 ; Saturation Check (-4)
cpfsgt ADL ; if ADL <= -4
retlw .252 ; Saturation: return -4
movf ADL,W ; else
return ; return Error
Greater
;;;;;;;;;;;;;;;;;;;;;;;;; Telemetry
movff ADL,ANGLE
movlw .127
cpfslt ANGLE ; if ANGLE > 127
movwf ANGLE ; ANGLE = 127
;;;;;;;;;;;;;;;;;;;;;;;;;
movlw .4 ; Saturation Check
cpfslt ADL ; if ADL >= 4
retlw .4 ; Saturation: return 4
movf ADL,W ; else
return ; return error
OutGreater
;;;;;;;;;;;;;;;;;;;;;;;;; Telemetry
movlw .127
movwf ANGLE ; ANGLE = 127
;;;;;;;;;;;;;;;;;;;;;;;;;
retlw .4 ; Saturation: return 4
OutLower
;;;;;;;;;;;;;;;;;;;;;;;;; Telemetry
movlw .128
movwf ANGLE ; ANGLE = -128
;;;;;;;;;;;;;;;;;;;;;;;;;
retlw .252 ; Saturation: return -4DESCRIPCION DE LA RUTINA DE CONTROL
La rutina comienza comprobando el estado del sistema ya que si se ha alcanzado alguno de los extremos del desplazamiento o se está calibrando la vertical no se realizará movimiento alguno a través del motor paso a paso aunque sí se enviará la información del estado a través de la telemetría.
A continuación se presenta el esquema de funcionamiento de la rutina de control y para una mejor comprensión se analizará cada grupo de acciones individualmente.
- Comprobaciones Iniciales
- Control Proporcional Derivativo
- Cálculo de la Velocidad
- Movimiento Motor Paso a Paso
COMPROBACIONES INICIALES
En el listado se detallan las instrucciones que ejecutan las comprobaciones iniciales. Primero se comprueba que no se hayan activado los finales de carrera (entradas RA6 y RA7 del puerto A), si es el caso, de registra este estado en la variable ENDS y se salta hacia la etiqueta Move0 para evitar que el cabezal se mueva. En caso contrario se comprueba el modo calibración, ya que si se está ajustando la verticalidad del péndulo, tampoco debe moverse el cabezal por motivos obvios.
Si no se da ninguna de estas condiciones la rutina continua su ejecución normal, pasando al cálculo del control proporcional derivativo.
;;; Test Ends
btfss PORTA,RA6 ; if left end is reached (RA6=0)
bsf ENDS,0 ; turn on ENDS<0>
btfss PORTA,RA7 ; if right end is reached (RA7=0)
bsf ENDS,1 ; turn on ENDS<1>
btfsc ENDS,0 ; if Left End
goto Move0 ; don't move head
btfsc ENDS,1 ; if Right End
goto Move0 ; don't move head
btfss PORTA,RA1 ; if calibrating (RA1=0)
goto Move0 ; don't move head
; else calculate movementCONTROL PROPORCIONAL DERIVATIVO
El bloque de control comienza calculando el diferencial entre el error actual y el de la iteración anterior guardándolo en la variable ERR_DIF que será utilizada luego para calcular la componente diferencial del control. Seguidamente se guarda el error actual en la variable ERR_1 para ser utilizado en la próxima iteración y se comienza el cálculo de la acción proporcional del control, véase el listado al final del apartado.
El control proporcional obtiene la constante de proporcionalidad KP desde las entradas RA2 y RA3 del puerto A, valor que puede ser modificado durante la ejecución desde el dip switch dispuesto para tal fin. El valor de KP, que podrá ser de 1, 2, 3 ó 4, se multiplica por el error, el cual al variar entre ±4 dará como resultado una excursión máxima del valor de la acción proporcional de ±16.
El control derivativo obtiene el valor de su constante KD desde las entradas RA4 y RA5 pertenecientes al puerto A también controladas por el dip switch. En este caso KD puede tomar el valor de 0, 1, 2 ó 3 permitiendo anularse la acción derivativa si KD toma el valor 0. Con el valor obtenido para KD se realiza la multiplicación por el error diferencial calculado anteriormente (guardado en ERR_DIF), se lo divide por dos para tener un control más suave sobre KD y finalmente se lo suma a la acción proporcional, obteniéndose así en la variable ACTION la suma de ambas acciones calculadas.
Teniendo en cuenta el valor máximo para el diferencial del error y el de la constante KD, la acción diferencial se encontrará entre ±24. Con esto el rango de valores que puede tomar ACTION será de ±40.
Para finalizar se guarda el valor de la acción calculada en la variable TEL_ACT para enviarla como telemetría y se calcula la dirección en la que se moverá el cabezal dependiendo del signo de la variable ACTION. Cabe destacar que para el siguiente apartado en dónde se calcula la velocidad con la que se moverá el motor paso a paso, ACTION debe ser siempre positivo por lo que si es necesario se le aplica el valor absoluto también dentro de este bloque de instrucciones.
;;; Calculate ERR_DIF
movf ERR_1,W
subwf ERR,W
movwf ERR_DIF ; ERR(k) - ERR(k-1) -> ERR_DIF
;;; Save current error
movff ERR,ERR_1 ; ERR(k) -> ERR(k-1)
Control
;;;;;;;;;;;;;;;;;;;;;;;;; Proportional Control
movlw B'00001100' ; Mask b2 and b3
andwf PORTA,W ; KP
rrncf WREG
rrncf WREG
incf WREG ; KP (1,2,3 or 4)
;;;;;;;;;;;;;;;;;;;;;;;;; Action Proportional
mulwf ERR ; KP * ERR(k) -> PRODH:PRODL
movff PRODL,ACTION ; Action P in ACTION [-16 - +16]
;;;;;;;;;;;;;;;;;;;;;;;;; Derivative Control
movlw B'00110000' ; Mask b4 and b5
andwf PORTA,W ; KD
rrncf WREG
rrncf WREG
rrncf WREG
rrncf WREG ; KD (0,1,2 or 3)
;;;;;;;;;;;;;;;;;;;;;;;;; Action Derivative
mulwf ERR_DIF ; KD * [ERR(k) - ERR(k-1)] -> PRODH:PRODL
btfsc ERR_DIF,7
subwf PRODH
movf PRODL,W
rlcf PRODH ; sign PRODH:PRODL -> carry
rrcf WREG ; Divides by 2 (KD = 0, 0.5, 1, 1.5)
addwf ACTION
movff ACTION,TEL_ACT ; Telemetry
;;;;;;;;;;;;;;;;;;;;;;;;; Resolve direction
movlw 0x00
cpfseq ACTION ; if ACTION <> 0
goto Direction ; goto Direction: move head
goto Move0 ; else No Error: Do not move head
Direction
movlw _DIR_RIGHT
movwf DIRECTION ; Move to the right by default
btfss ACTION,7 ; if ACTION >= 0
goto Quantizer ; goto Quantizer
;;; Positivate
negf ACTION ; -ACTION -> ACTION
movlw _DIR_LEFT
movwf DIRECTION ; _DIR_LEFT -> DIRECTION
CÁLCULO DE LA VELOCIDAD
Con el valor absoluto de la acción proporcional derivativa en la variable ACTION toca calcular a que velocidad se desplazará el motor para corregir la inclinación del péndulo. Experimentalmente se ha comprobado que para este tipo de motor existen 4 niveles útiles de velocidad a partir de los cuales el movimiento no provoca efecto correctivo alguno sobre el péndulo.
Teniendo en cuenta esta restricción se calcula la velocidad para el motor como ciclos de retardo y se guarda este valor en la variable DELAY como se puede comprobar en el listado a continuación. Luego este retardo será utilizado por el bloque de movimiento en el siguiente apartado para incrementar la fase de alimentación del motor paso a paso.
Quantizer
;;;;;;;;;;;;;;;;;;;;;;;;; Quantize in 4 levels
movff ACTION,DELAY
bsf STATUS,C ; Set Carry
movlw .5
subfwb DELAY ; 5 - DELAY -> DELAY
btfsc DELAY,7 ; if DELAY < 0
clrf DELAY ; 0 -> DELAY
movlw .0
cpfseq DELAY
goto Dir ; if DELAY = 0
movlw .1
movwf DELAY ; 1 -> DELAY
MOVIMIENTO MOTOR PASO A PASO
Este bloque consta de dos secciones, la primera se encarga de llevar la cuenta de los ciclos de ejecución de la rutina de control en la variable COUNT que al ser comparada con el valor de DELAY determinará si el motor avanza un paso en este ciclo o no, controlando así la velocidad de giro que variará entre 6.76giros/seg y 1.69giros/seg.
La segunda sección comprueba el valor actual para la secuencia de movimiento del motor paso a paso almacenada en la variable SEQ pasando a la siguiente o anterior para provocar el avance o retroceso del mismo una vez que SEQ sea volcada en el puerto B.
En el Listado siguiente se observa la porción de código en dónde se ha implementado este código, en dónde también se guarda el sentido de movimiento en la variable HEAD para ser transmitida luego como telemetría vía el puerto serie RS232.
Dir
incf COUNT ; Inc COUNT
movf DELAY,W
cpfsgt COUNT ; if COUNT <= DELAY
goto Move0 ; do nothing (Stop & Wait)
movlw 0x01
movwf COUNT ; else begin count again
; Resolves direction of movement
movlw _DIR_LEFT
cpfseq DIRECTION ; if DIRECTION equals DIR_LEFT
goto Right ; jump to Right
goto Left ; else jump to Left
Left ; Moves to the Left
movlw 'L'
movwf HEAD ; Telemetry: Move Head to the Left
incf SEQ ; Inc SEQ
movlw 0x04
cpfsgt SEQ ; if SEQ <= 4
goto Move ; jump to Move
movlw 0x01
movwf SEQ ; else SEQ = 1
goto Move ; jump to Move
Right ; Moves to the Right
movlw 'R'
movwf HEAD ; Telemetry: Move Head to the Right
decfsz SEQ ; Dec SEQ, if SEQ <> 0
goto Move ; Jump to Move
movlw 0x04
movwf SEQ ; else SEQ = 4
Move ; moves head
movlw 0x01
cpfseq SEQ ; if SEQ <> 1
goto Move2 ; go to test for _SEQ_2
movlw _SEQ_1 ; else w = _SEQ_1
goto EndMove
Move2
movlw 0x02
cpfseq SEQ ; if SEQ <> 2
goto Move3 ; go to test for _SEQ_3
movlw _SEQ_2 ; else w = _SEQ_2
goto EndMove
Move3
movlw 0x03
cpfseq SEQ ; if SEQ <> 3
goto Move4 ; go to test for _SEQ_3
movlw _SEQ_3 ; else w = _SEQ_3
goto EndMove
Move4
movlw _SEQ_4 ; w = _SEQ_4
goto EndMove
Move0
movlw 'S'
movwf HEAD ; Telemetry: Stop Head
movlw 0x00 ; remove voltage of motor
EndMove
movwf PORTB ; puts W in PORTB
