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