            TITLE   FUZZY CONTROL ROUTINES

LIMIT       EQU     29

* this module defines the membership functions for ERR and DERIV
* and computes VOUT, according to the MIN-MAX algorithm
* this code is executed every 60ms.
            CODE

            TST     T4MS1
            JNE     END_FUZZY
            LDAA    #15
            STAA    T4MS1
            JSR     CAT         ;create antecendent table
            JSR     CRISP       ;compute crisp value of VOUT
            JMP     END_FUZZY
********************************************************************
* CAT - subroutine to create the antecedent table starting from
* the current values of ERR and DERIV
********************************************************************
CAT         LDD     #$FFFF          ;check sign of ERR and DERIV
            STD     TEMP
            LDD     ERR
            BMI     CAT01
            CLR     TEMP
CAT01       LDD     DERIV
            BMI     CAT02
            CLR     TEMP+1
CAT02       LDD     TEMP
            ANDA    #$02
            STAA    TEMP
            ANDB    #$01
            ORAB    TEMP
            LSLB
            LDX     #JTAB
            ABX
            LDX     0,X
            JMP     0,X
* ERR>=0, DERIV>=0
CAT10       LDD     ERR
            CMPB    #LIMIT
            BLO     CAT11
            LDAB    #LIMIT
            STD     ERR
CAT11       LDD     DERIV
            CMPB    #LIMIT
            BLO     CAT12
            LDAB    #LIMIT
            STD     DERIV

CAT12       LDX     #MFEN1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT13
            LDAA    TEMP
CAT13       LDX     #ATAB
            STAA    0,X

CAT14       LDX     #MFEZ1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT15
            LDAA    TEMP
CAT15       LDX     #ATAB
            STAA    1,X

CAT16       LDX     #MFEP1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT17
            LDAA    TEMP
CAT17       LDX     #ATAB
            STAA    2,X

CAT18       LDX     #MFEN1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT19
            LDAA    TEMP
CAT19       LDX     #ATAB
            STAA    3,X

CAT20       LDX     #MFEZ1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT21
            LDAA    TEMP
CAT21       LDX     #ATAB
            STAA    4,X

CAT22       LDX     #MFEP1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT23
            LDAA    TEMP
CAT23       LDX     #ATAB
            STAA    5,X

CAT24       LDX     #MFEN1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT25
            LDAA    TEMP
CAT25       LDX     #ATAB
            STAA    6,X

CAT26       LDX     #MFEZ1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT27
            LDAA    TEMP
CAT27       LDX     #ATAB
            STAA    7,X

CAT28       LDX     #MFEP1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT29
            LDAA    TEMP
CAT29       LDX     #ATAB
            STAA    8,X
            RTS

* ERR>=0, DERIV<0
CAT30       LDD     DERIV
            COMA    
            COMB
            ADDD    #1
            STD     DERIV
            LDD     ERR
            CMPB    #LIMIT
            BLO     CAT31
            LDAB    #LIMIT
            STD     ERR
CAT31       LDD     DERIV
            CMPB    #LIMIT
            BLO     CAT32
            LDAB    #LIMIT
            STD     DERIV

CAT32       LDX     #MFEN1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT33
            LDAA    TEMP
CAT33       LDX     #ATAB
            STAA    0,X

CAT34       LDX     #MFEZ1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT35
            LDAA    TEMP
CAT35       LDX     #ATAB
            STAA    1,X

CAT36       LDX     #MFEP1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT37
            LDAA    TEMP
CAT37       LDX     #ATAB
            STAA    2,X

CAT38       LDX     #MFEN1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT39
            LDAA    TEMP
CAT39       LDX     #ATAB
            STAA    3,X

CAT40       LDX     #MFEZ1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT41
            LDAA    TEMP
CAT41       LDX     #ATAB
            STAA    4,X

CAT42       LDX     #MFEP1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT43
            LDAA    TEMP
CAT43       LDX     #ATAB
            STAA    5,X

CAT44       LDX     #MFEN1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT45
            LDAA    TEMP
CAT45       LDX     #ATAB
            STAA    6,X

CAT46       LDX     #MFEZ1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT47
            LDAA    TEMP
CAT47       LDX     #ATAB
            STAA    7,X

CAT48       LDX     #MFEP1   
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT49
            LDAA    TEMP
CAT49       LDX     #ATAB
            STAA    8,X
            RTS

* ERR<0, DERIV>=0
CAT50       LDD     ERR
            COMA
            COMB
            ADDD    #1
            STD     ERR
            CMPB    #LIMIT
            BLO     CAT51
            LDAB    #LIMIT
            STD     ERR
CAT51       LDD     DERIV
            CMPB    #LIMIT
            BLO     CAT52
            LDAB    #LIMIT
            STD     DERIV

CAT52       LDX     #MFEN2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT53
            LDAA    TEMP
CAT53       LDX     #ATAB
            STAA    0,X

CAT54       LDX     #MFEZ2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT55
            LDAA    TEMP
CAT55       LDX     #ATAB
            STAA    1,X

CAT56       LDX     #MFEP2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT57
            LDAA    TEMP
CAT57       LDX     #ATAB
            STAA    2,X

CAT58       LDX     #MFEN2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT59
            LDAA    TEMP
CAT59       LDX     #ATAB
            STAA    3,X

CAT60       LDX     #MFEZ2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT61
            LDAA    TEMP
CAT61       LDX     #ATAB
            STAA    4,X

CAT62       LDX     #MFEP2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT63
            LDAA    TEMP
CAT63       LDX     #ATAB
            STAA    5,X

CAT64       LDX     #MFEN2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT65
            LDAA    TEMP
CAT65       LDX     #ATAB
            STAA    6,X

CAT66       LDX     #MFEZ2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT67
            LDAA    TEMP
CAT67       LDX     #ATAB
            STAA    7,X

CAT68       LDX     #MFEP2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP1
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT69
            LDAA    TEMP
CAT69       LDX     #ATAB
            STAA    8,X
            RTS

* ERR<0, DERIV<0
CAT70       LDD     ERR
            COMA    
            COMB
            ADDD    #1
            STD     ERR
            LDD     DERIV
            COMA    
            COMB
            ADDD    #1
            STD     DERIV
            LDD     ERR
            CMPB    #LIMIT
            BLO     CAT71
            LDAB    #LIMIT
            STD     ERR
CAT71       LDD     DERIV
            CMPB    #LIMIT
            BLO     CAT72
            LDAB    #LIMIT
            STD     DERIV

CAT72       LDX     #MFEN2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT73
            LDAA    TEMP
CAT73       LDX     #ATAB
            STAA    0,X

CAT74       LDX     #MFEZ2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT75
            LDAA    TEMP
CAT75       LDX     #ATAB
            STAA    1,X

CAT76       LDX     #MFEP2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDN2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT77
            LDAA    TEMP
CAT77       LDX     #ATAB
            STAA    2,X

CAT78       LDX     #MFEN2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT79
            LDAA    TEMP
CAT79       LDX     #ATAB
            STAA    3,X

CAT80       LDX     #MFEZ2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT81
            LDAA    TEMP
CAT81       LDX     #ATAB
            STAA    4,X

CAT82       LDX     #MFEP2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDZ2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT83
            LDAA    TEMP
CAT83       LDX     #ATAB
            STAA    5,X

CAT84       LDX     #MFEN2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT85
            LDAA    TEMP
CAT85       LDX     #ATAB
            STAA    6,X

CAT86       LDX     #MFEZ2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT87
            LDAA    TEMP
CAT87       LDX     #ATAB
            STAA    7,X

CAT88       LDX     #MFEP2
            LDD     ERR
            ABX
            LDAA    0,X
            STAA    TEMP
            LDX     #MFDP2
            LDD     DERIV
            ABX
            LDAA    0,X
            CMPA    TEMP
            BHS     CAT89
            LDAA    TEMP
CAT89       LDX     #ATAB
            STAA    8,X
            RTS


* compute crisp value of VOUT

CRISP       LDD     #0
            STD     AVG
            LDAA    #9          ;counter
            STAA    TEMP
            LDX     #ATAB
            LDY     #SGTAB
CR10        LDAA    0,X
            LDAB    0,Y            
            MUL
            ADDD    AVG
            STD     AVG
            DEC     TEMP
            BNE     CR10
CR20        LDD     #0
            STD     SUMZ
            LDAA    #9
            STAA    TEMP
            LDX     #ATAB
CR25        CLRA
            LDAB    0,X
            ADDD    SUMZ
            STD     SUMZ
            DEC     TEMP
            BNE     CR25
            LDD     AVG
            LDX     SUMZ
            IDIV
            STX     VOUT
            RTS

************************************************************
JTAB        DW      CAT10
            DW      CAT30
            DW      CAT50
            DW      CAT70

END_FUZZY   EQU     *

            END
