; ********* Fuzzy.s ******************

; DC motor, Fuzzy Logic control

; Last modified 1/16/02 by Jonathan W. Valvano

 

; Copyright 2002 by Jonathan W. Valvano, valvano@uts.cc.utexas.edu 

;    You may use, edit, run or distribute this file 

;    as long as the above copyright notice remains 



           .area bss

; Fuzzy Membership sets

; input membership variables

fuzvar:: .blkb 8  ; inputs

veryfast=  0    ; speed very fast

fast    =  1    ; speed too fast

ok      =  2    ; speed OK

slow    =  3    ; speed too slow

veryslow=  4    ; speed very slow

up      =  5    ; speed increasing

same    =  6    ; speed constant

down    =  7    ; speed decreasing

;output membership variables

fuzout:: .blkb 5 ; outputs   

bigadd  =  8    ; add a lot of power to system

add     =  9    ; add some power to system

zero    =  10   ; leave power as is

sub     =  11   ; subtract some power from system

bigsub  =  12   ; subtract some power from system



	     .area text

s_tab::  .byte  $B0,$FF,$08,$00   ; very fast

         .byte  $80,$D0,$08,$08   ; fast

         .byte  $60,$A0,$08,$08   ; OK

         .byte  $30,$80,$08,$08   ; slow

         .byte  $00,$50,$00,$08   ; very slow

a_tab::  .byte  $80,$FF,$08,$00   ; increasing speed

         .byte  $60,$A0,$08,$08   ; constant speed

         .byte  $00,$80,$00,$08   ; decreasing speed

rules::

         .byte  veryfast,up,$FE,bigsub,$FE

         .byte  veryfast,same,$FE,bigsub,$FE

         .byte  veryfast,down,$FE,sub,$FE

         .byte  fast,up,$FE,sub,$FE  

         .byte  fast,same,$FE,sub,$FE

         .byte  fast,down,$FE,zero,$FE  

         .byte  ok,up,$FE,sub,$FE  

         .byte  ok,same,$FE,zero,$FE  

         .byte  ok,down,$FE,add,$FE  

         .byte  slow,up,$FE,zero,$FE  

         .byte  slow,same,$FE,add,$FE  

         .byte  slow,down,$FE,add,$FE  

         .byte  veryslow,up,$FE,add,$FE

         .byte  veryslow,same,$FE,bigadd,$FE  

         .byte  veryslow,down,$FE,bigadd,$FE

         .byte  $FF  

addsingleton:: 

    .byte 128+15,128+8,128,128-8,128-15  ; 128 subtracted, +15,+8,0,-8,-15



; RegB=SpeedError, 0 to 255, 128 means no error

; Stack parameter 5,sp is Acceleration, 0 to 255, 128 means no acceleration

; return dU, -128 to +127, 0 means no change    

_FuzzyLogic::  

    pshx      

    tba      ; RegA=SpeedError, 0 to 255

    ldx  #s_tab

    ldy  #fuzvar

    mem

    mem

    mem

    mem

    mem



    ldaa 5,sp   ;RegA=Acceleration, 0 to 255

    ldx  #a_tab

    mem

    mem

    mem



    ldab #5     ; number of output fuzzy membership sets

cloop:

    clr  1,y+   ; clear output fuzzy variables

    dbne b,cloop

    ldx  #rules

    ldy  #fuzvar

    ldaa #$FF

    rev

    ldy  #fuzout

    ldx  #addsingleton

    ldab #5

    wav

    ediv         ; Y has weighted average, 0 to 255

    leay -128,y  ; adjust for offset, -128 to +127

    tfr  y,d     ; return dpower

    pulx

    rts