English

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.

Esquemático del Controlador
Esquemático del Controlador
Montaje Para los Ensayos en la Protoboard
Montaje Para los Ensayos en la Protoboard

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.

Esquema de Conexionado del Controlador
Esquema de Conexionado del Controlador
Detalle del Montaje del Potenciómetro
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.

Detalle de los Interruptores de Final de Carrera
Detalle de los Interruptores de Final de Carrera

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:

Estructura del Programa del Controlador
Estructura del Programa del Controlador

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
  ;;;;;;;;;;;;;;;;;;;;;;;;;  
Código de Inicialización

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.

Ajuste fino para la puesta a 0
Ajuste fino para la puesta a 0

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.

Estructura de la Rutina de Lectura y Acondicionamiento del Error
Estructura de la Rutina de Lectura y Acondicionamiento del Error

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 -4
Lectura y Acondicionamiento del Error

DESCRIPCION 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.

Rutina de control
Rutina de control

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 movement
Comprobaciones Iniciales

CONTROL 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
Control Proporcional Derivativo

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
Cálculo de la Velocidad

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
Movimiento Motor Paso a Paso
Licencia Creative Commons