            TITLE   COMPUTE VOUT

* This code is executed every 20 ms
* If ERR <0 then VOUT=0 (Temperature higher than set-point pemperature)
* If ERR >= PB, then VOUT=$00FF (VMAX)
* If (VOUT+INTEGRAL) < 0, then VOUT=KP*ERR
* Else, compute VOUT=KP*(ERR+INTEGRAL). If VOUT>$FF then VOUT=$00FF (VMAX).

PB          EQU     15              ;proportional band

            CODE

            TST     T4MS2
            BNE     END_CTRL
            LDAA    #5
            STAA    T4MS2           ;reload counter

            LDD     ERR
            BMI     CTRL90          ;VOUT=0
            CPD     #PB
            BHI     CTRL99          ;VOUT=$FF
            ADDD    INTEGRAL        ;D=(ERR+INTEGRAL)
            BMI     CTRL10
            LDAA    KP
            MUL                     ;D=KP*(ERR+INTEGRAL)
            CPD     #$00FF
            BHS     CTRL99
            STD     VOUT
            BRA     END_CTRL
CTRL10      LDD     ERR             
            LDAA    KP
            MUL
            STD     VOUT
            BRA     END_CTRL
CTRL90      LDD     #0
            STD     VOUT
            BRA     END_CTRL
CTRL99      LDD     #$00FF
            STD     VOUT
END_CTRL    EQU     *

            END


