            TITLE   INTEGRAL CONTROL

* this module is executed every 100ms

            CODE

            TST     T4MS1
            BNE     END_INT
            LDAA    #25             ;reload timer
            STAA    T4MS1
* Update IBUF
            LDD     ERR
            LDX     XIBUF
            STD     0,X
            INX
            INX
            CPX     #ENDIBUF
            BHS     INT10
            STX     XIBUF
            BRA     INT20
INT10       LDX     #IBUF
            STX     XIBUF
* compute the average of the last TI values stored in IBUF
INT20       LDD     #0
            STD     AVG
            LDAB    TI
            LDX     XIBUF
INT30       DEX
            DEX
            CPX     #IBUF
            BLO     INT40
            LDD     0,X
            ADDD    AVG
            DECB
            BNE     INT30
            BRA     INT50
INT40       LDX     #ENDIBUF       
            BRA     INT30
* now AVG contains the sum of the last (TI) values of ERR
INT50       LDD     AVG         ;AVG is a SIGNED integer!
            BMI     INT60
            LDX     #0
            LDAB    TI
            ABX                 ;prepare for IDIV
            IDIV                ;D:X->X (D contains the reaminder)
            STX     INTEGRAL
* when AVG is negative, compute 2's complement before IDIV
INT60       COMA
            COMB
            ADDD    #1
            LDX     #0
            LDAB    TI
            ABX                 ;prepare for IDIV
            IDIV                ;D:X->X (D contains the reaminder)
            TDX                 ;put result in D
            COMA
            COMB
            ADDD    #1
            STD     INTEGRAL
END_INT     EQU     *

