;
;Fuzzy logic controller
;Written by Lindsay Meek
;http://www.kitstream.com
;
;This software is freeware and is provided AS-IS with no warranties
;

;Based on the freeware motorola 'fudge' engine for the HC11 
;
;Achieves about a 1ms execution time on a PIC16F876 @ 16 MHz with two inputs & outputs
;

                    include     "p16f876.inc"

#define _c      STATUS,C
#define _z      STATUS,Z
#define _rp0    STATUS,RP0
#define _rp1    STATUS,RP1


;
;Banked subroutine call
;
bcall   macro   addr
        local   here
        lcall   addr
here:
        if  here & 0x800
        bsf PCLATH,3
        else
        bcf PCLATH,3
        endif
        if  here & 0x1000
        bsf PCLATH,4
        else
        bcf PCLATH,4
        endif
        endm

    org     0

    goto    RESET

divinner16: macro
    local   nocarry
    rlf     sum_of_prod,f
    rlf     sum_of_prod+1,f
    rlf     L_byte,f
    rlf     H_byte,f
    movfw   sum_of_fuz
    subwf   L_byte,w
    movfw   sum_of_fuz+1
    btfss   _c
    addlw   1
    subwf   H_byte,w
    btfss   _c
    goto    nocarry
    movwf   H_byte
    movfw   sum_of_fuz
    subwf   L_byte,f
    bsf _c
nocarry:
    endm

divinner24: macro
    local   nocarry
    rlf     sum_of_prod,f
    rlf     sum_of_prod+1,f
    rlf     sum_of_prod+2,f
    rlf     L_byte,f
    rlf     H_byte,f
    movfw   sum_of_fuz
    subwf   L_byte,w
    movfw   sum_of_fuz+1
    btfss   _c
    addlw   1
    subwf   H_byte,w
    btfss   _c
    goto    nocarry
    movwf   H_byte
    movfw   sum_of_fuz
    subwf   L_byte,f
    bsf _c
nocarry:
    endm

div16x15:

    clrf    L_byte
    clrf    H_byte
    bcf _c

        ;simple speedup. if denominator > 8 bits [typical case], then first 8 loops will do nothing
        ;and they can be eliminated

        movfw   sum_of_fuz+1
        btfsc   _z
        goto    no_div_opt
        
        movfw   sum_of_prod+1
        movwf   L_byte
        movfw   sum_of_prod
        movwf   sum_of_prod+1
        clrf    sum_of_prod
        goto    div_opt

no_div_opt:

    divinner16
    divinner16
    divinner16
    divinner16
    divinner16
    divinner16
    divinner16
    divinner16

div_opt:

    divinner16
    divinner16
    divinner16
    divinner16
    divinner16
    divinner16
    divinner16
    divinner16

    rlf     sum_of_prod,f     
    rlf     sum_of_prod+1,f
    movfw   sum_of_prod
    return

div24x15:

    clrf    L_byte
    clrf    H_byte
    bcf _c

        ;simple speedup. if denominator > 8 bits [typical case], then first 8 loops will do nothing
        ;and they can be eliminated

        movfw   sum_of_fuz+1
        btfsc   _z
        goto    no_div_opt2
        
        movfw   sum_of_prod+2
        movwf   L_byte
        movfw   sum_of_prod+1
        movwf   sum_of_prod+2
        movfw   sum_of_prod
        movwf   sum_of_prod+1
        clrf    sum_of_prod
        goto    div_opt2

no_div_opt2:
    
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24

div_opt2:

        divinner24
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24

        divinner24
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24
        divinner24

        rlf     sum_of_prod,f       
        rlf     sum_of_prod+1,f
        rlf     sum_of_prod+2,f
        movfw   sum_of_prod
        return

normalise24:    macro   var,shifts

    local   i=0

    while   i<shifts
    bcf _c
    rrf var+2,f
    rrf var+1,f
    rrf var+0,f
i += 1

    endw
    endm

;
;Main line
;
RESET:  
   
    bcf     _rp0

Loop:
    clrwdt
   
    movlw   0                   ;raw input #1
    movwf   current_ins+0

    movlw   0                   ;raw input #2
    movwf   current_ins+1

    movlw   0
    movwf   current_ins+2       ;raw input #3

    bcall   FuzzyEngine

    movfw   cog_outs+0          ;raw output #1

    movfw   cog_outs+1          ;raw output #2

    movfw   cog_outs+2          ;raw output #3

    goto    Loop

FUZ_BASE    equ    0x800

    org     FUZ_BASE

;
;output from fudge, coverted to PIC assembler syntax with 'conv'
;
    include     "rules.asm"


;
;indirectly access a value in the fuzzy logic tables
;
LUT:    movwf       PCL

;
;macro to evaluate an input against a membership function
;
member_eval:    macro   member_ptr,input
        local   hav_grad,not_seg2,no_fix,vert_slp,use_b

    movlw       member_ptr+2
    call        LUT     ;point 2
    
    subwf       input,w
    btfss       _c
    goto        not_seg2
    btfsc       _z
    goto        not_seg2

    movwf       mulplr      ;if input < point 2

    movlw       member_ptr+3
    call        LUT     ;slope 2

    xorlw       0
    btfsc       _z
    goto        hav_grad    ;skip if vertical slope
    
    call        mpy     ;slope * (input - point2)

    btfsc       _z      ;test if upper 8 bits = 0
    goto        no_fix

    clrw                ;no, limit grade to zero
    goto        hav_grad

no_fix:

    movlw       .255
    subwf       L_byte,w    ;sub 0xff
    sublw       0       ;neg
    goto        hav_grad

not_seg2:

    movlw       member_ptr+0
    call        LUT     ;point 1
    
    subwf       input,w

    movwf       mulplr

    clrw
    btfss       _c
    goto        hav_grad    ;< point 1 so grade=0

    movlw       member_ptr+1
    call        LUT     ;slope 1

    xorlw       0
    btfsc       _z
    goto        vert_slp    ;skip if vertical slope

    call        mpy     ;slope * (input-point1)

    btfsc       _z      ;see if upper 8 bits = 0
    goto        use_b       ;yes, use result 

vert_slp:

    movlw       .255        ;vertical slope encountered
    goto        hav_grad

use_b:

    movfw       L_byte

hav_grad:
    endm

;
;macro to unroll all possible membership function tests
;
fuzzify_inputs: macro   input_vectors,member_ptr,input
        
    local   i,j
   
j=0

    movlw   input_vectors
    movwf   FSR

    while   j<NUMINP

i=0

    while   i<8

    member_eval member_ptr+(i+j*8)*4,input+j

    movwf   INDF
    incf    FSR,f

i += 1
    endw    
j += 1
    endw
    endm

;
;macro to clear fuzzy outputs
;
clear_outputs:  macro   output
        
    local   i=0
    while   i<8*NUMOUT
    clrf    output+i
i += 1
    endw
    endm

defuzzify_inner:    macro   output,weight

    local   is_zero

    movfw   output
    btfsc   _z
    goto    is_zero

    movwf   mulplr
    addwf   sum_of_fuz,f
    btfsc   _c
    incf    sum_of_fuz+1,f  ;sum outputs

    movlw   weight
    call    LUT
    xorlw   0       ;ignore zero weights
    btfsc   _z
    goto    is_zero

    call    mpy     ;position times weight

    movfw   L_byte
    addwf   sum_of_prod,f
    movlw   1
    btfsc   _c
    addwf   sum_of_prod+1,f
    btfsc   _c
    addwf   sum_of_prod+2,f
    movfw   H_byte
    addwf   sum_of_prod+1,f
    btfsc   _c
    incf    sum_of_prod+2,f 

is_zero:
    endm

defuzzify_output:   macro   output,weights,crisp,index  

    local   i=0,num_big,sav_out,is_zero

    clrf    sum_of_fuz
    clrf    sum_of_fuz+1
    clrf    sum_of_prod
    clrf    sum_of_prod+1
    clrf    sum_of_prod+2

    while   i<8

    defuzzify_inner output+i+index*8,weights+i+index*8

i += 1
    endw

    movfw   sum_of_fuz
    iorwf   sum_of_fuz+1,w
    btfsc   _z              ;denominator zero?
    goto    sav_out         ;yes, output is zero then

    movfw   sum_of_prod+2
    btfss   _z              ;numerator > 16 bit?
    goto    num_big
    
    bcall   div16x15        ;crisp = sum(weights*outputs)/sum(outputs)

    goto    sav_out

num_big:    
    bcall    div24x15
   
sav_out:
    movwf   crisp+index     ;output table

    movlw   high FUZ_BASE
    movwf   PCLATH

    endm

;
;macro to convert fuzzy inferences to crisp outputs
;
defuzzify:  macro   output,weights,crisp

    local   j=0

    while   j<NUMOUT

    defuzzify_output    output,weights,crisp,j

j += 1
    endw

    endm


;
;****   Define a macro for adding & right shifting  **
;
mult    MACRO   bit             ; Begin macro
	btfsc   mulplr,bit
        addwf   H_byte,f   
        rrf     H_byte,f   
        rrf     L_byte,f   
	ENDM                    ; End of macro
;
; *****************************         Begin Multiplier Routine
;
; Inputs:
;
; mulplr = multiplier
; W=mulcnd
;
; Outputs:
;
; L_byte,H_byte result
;
; W=H_byte

mpy     clrf    H_byte
		clrf    L_byte
        bcf     STATUS,C             ; Clear the carry bit in the status Reg.
		mult    0
		mult    1
		mult    2
		mult    3
		mult    4
		mult    5
		mult    6
		mult    7
        movfw   H_byte
        return
  
;
;Fuzzy logic controller. Based on the motorola 'Fudge' engine
;
;Inputs     current_ins crisp inputs
;Outputs    cog_outs    crisp outputs
;
FuzzyEngine:

    movlw   high FUZ_BASE
    movwf   PCLATH

    fuzzify_inputs  fuz_ins,INPUT_MFS-FUZ_BASE,current_ins
    
    clear_outputs   fuz_outs

    ;process rules

    movlw   RULE_START-FUZ_BASE
    movwf   addr

    movlw   .255
    movwf   grade       ;reset highest membership grade
if_loop:
    movfw   addr
    call    LUT         ;read rule table entry
    movwf   FSR         ;save rule
    incf    addr,f      ;advance to next rule
    btfsc   _z
    incf    PCLATH,f
    btfsc   FSR,7       ;'then' or 'if'?
    goto    then_loop

got_if:
    movlw   fuz_ins     ;no, if
    addwf   FSR,f       ;calc input rule ptr

    movfw   INDF
    subwf   grade,w     ;is input rule less than highest grade?
    movfw   INDF
    btfsc   _c
    movwf   grade       ;yes, replace highest grade with input rule
    goto    if_loop 
    
if_loop_restart:
    movlw   .255
    movwf   grade       ;reset highest membership grade
    goto    got_if      ;process 'if'

then_loop:
    bcf     FSR,7       ;clear bit 7
    movlw   fuz_outs
    addwf   FSR,f       ;calc output rule ptr
    
    movfw   grade
    subwf   INDF,w      ;is grade higher than output?
    movfw   grade
    btfss   _c
    movwf   INDF        ;yes, replace output 

    movfw   addr
    call    LUT         ;get next rule
    movwf   FSR
    incf    addr,f
    btfsc   _z
    incf    PCLATH,f
    btfss   FSR,7
    goto    if_loop_restart 

    incfsz  FSR,w       ;end of rule set? (0xff)
    goto    then_loop   ;no, process next 'then'

    movlw   high FUZ_BASE
    movwf   PCLATH

    defuzzify   fuz_outs,SGLTN_POS-FUZ_BASE,cog_outs 
    return

    CBLOCK      0x20

    mulplr
    L_byte
    H_byte
    
    grade
    addr

    current_ins:NUMINP
    fuz_ins:8*NUMINP
    fuz_outs:8*NUMOUT
    cog_outs:NUMOUT
    sum_of_fuz:2
    sum_of_prod:3
  
    ENDC

    end