Daniel Stolfi's web page
EspañolEnglish

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
LA PAGINA DE DANIEL STOLFI - Version 5.09 - 1993-2024 - Cookies Policy
Creative Commons License