'                       THIS IS A PROTOTYPE PROGRAM
'                       NEXTING IS NOT COMPLETE
'
'Author:                Don Endres
'Last Rev Date:         7/23/93
'Written for:           Qbasic
'
'
'*****************************************************************************
'                       fuzcom1.txt
'*****************************************************************************
'This program is a fuzzy motor position control.  It will obtain input from the screen
'and call subroutines which contain drivers to read from and write to a DAQ
'card and compute the control value with a fuzzy algorithm.
'****************************************************************************
'                Subroutines:
DECLARE SUB anain (strtadd, inport#, inrange, invalu, test)
declare sub     anaout(, test)
DECLARE SUB fuzspeed (errval, velcty, outval, test)
'****************************************************************************
'       Variables used in main program:
'
'       delay           desired                 errval             inport#                 inrange
'       invalu          outport#                strtadd            test            velcty
'*****************************************************************************
test = 1                        'Setting test to 1 will install constants, bypass screen
IF test = 1 THEN                ' prompts & provide messages to speed up testing.
	strtadd = &H330
	inport# = 7
	inrange = 8
	outport# = 0
	desired = 2047
ELSE
	INPUT "     Enter base address of DAQ board in hex (ex: &H330)      ", strtadd
	INPUT "     Enter analog output port number (ex: 0 or 1)        ", outport#
	INPUT "     Enter first analog input port number (ex: 0, 1, .. 7)    ", inport#
	INPUT "     Enter input range value from page 16 (ex: 8, 0, 10, 12, 14)  ", inrange
	INPUT "     Enter desired motor position between 0 & 4095    ", desired
END IF
GOSUB anain                     'Get first position value
IF test = 1 THEN                'Provide info if testing
  PRINT "Returned from Anain (1st)!"
END IF
DO
	invalu1 = invalu        'Time shift the position data back one time interval
	GOSUB anain             ' Get the current position value
	IF test = 1 THEN        'Provide info if testing
	  PRINT "Returned from Anain (Loop)!"
	END IF
	errval = invalu - desired'Calculate how far to desired position
	velcty = invalu - invalu1'Calculate how fast the position is changing & the direction
	GOSUB fuzspeed
	IF test = 1 THEN         'Output if testing
	  PRINT "Back from fuzz", errval, desired, velcty, outvalu
	END IF
	GOSUB anaout             'Set output to outvalu
	IF test = 1 THEN         'Provide info if testing
	  PRINT "Returned from Anaout !"
	END IF
	delay = 0
	FOR delay = 1 TO 100000
	NEXT
LOOP
END



'****************************************************************************************************************
'****************************************************************************************************************
SUB anain
'-------------------------------------------------------------------------------------------------------------------------------
' Analog Input Subroutine
'
'This subroutine will read an input from the port specified in the
'variable and return a number in the range of 0 to 4095 representing its
'relative position in the input range.
'****************************************************************************************************************
'      VARIABLES PASSED IN                      *       VARIABLES  PASSED OUT
'       test    (0, 1)                          *
'       strtadd   (hex board address)           *       invalu   (between 0 & 4095)
'       inport#  (0 to 7)                               *
'       inrange  (8, 0, 10, 12 or 14)           *
'*************************************************************************************************************
'       VARIABLES USED IN THIS SUBROUTINE
'
'  channel      -->Input channel for which range is being set
'  rgconad        -->Address of range control register
'  inport#      -->Output port (0 or 1) (passed in from main program)
'  inrange      -->Control of desired range from the top chart on page 16 (8, 0, 10, 12, 14)
'  invalu       -->Input value in the range of 0 to 4095
'  adhb         -->High 4 bits of the AD conversion
'  adlb         -->Low 8 bits of the AD conversion
'  strtadd      -->Starting address of the board (passed in from main program)
'  status       -->Contents of Status & Control Register
' test   -->Flag to activate debug info
'***************************************************************************
if test =1                              'Display info to aid in testing
	PRINT "In analog input routine!"
END IF
rgconad = strtadd + 3                   'Define the address for the analog control register
OUT rgconad, (inrange)                  'Put the range setting in the control register
status = INP(strtadd + 2)               'Initialize the flag to match converter busy flag
DO WHILE status > 127                   'Check to see if the converter is currently busy
status = INP(strtadd + 2)               '     and wait until it is not
LOOP
OUT (strtadd + 2), (inport#)            'Set the MUX to the right channel
OUT (strtadd + 1), (0)                  'Start an A to D conversion
status = INP(strtadd + 2)               'Initialize the flag to match converter busy flag
DO WHILE status > 127                   'Check to see if the converter is currently busy
status = INP(strtadd + 2)               '     and wait until it is not
LOOP
adhb = INP(strtadd + 1)                 'Get the high byte of the converted value out of the register
adlb = INP(strtadd)                     'Get the low byte of the converted value out of the register
invalu = (adlb / 16) + (adhb * 16)      'Add the high and low bytes together with the proper multiper
endsub

'****************************************************************************************************************
'****************************************************************************************************************

SUB anaout
'-------------------------------------------------------------------------------------------------------------------------------
'Analog Output Subroutine
'
'This subroutine will send an output to the port specified in the
'variable outport# of the value specified in the variable outvalu.
'****************************************************************************************************************
'      VARIABLES TO PASS IN     *         VARIABLES TO  PASS OUT
'                               *
'  strtadd   (hex board address)*
'  outport#  (0,1)              *
'  outvalu   (between 0 & 4095) *
'****************************************************************************************************************
'       VARIABLES USED IN THIS PROGRAM
'
'  hb           -->Decimal value of the high 4 bits
'  offaddhb     -->Address of the high 4 bits of the AD conversion
'  offaddlb     -->Address of the low 8 bits of the AD conversion
'  outport#     -->Output port (0 or 1) (passed in from main program)
'  outvalu      -->Desired output level (0 to 4095) (passed in from main program)
'  strtadd      -->Starting address of the board (passed in from main program)
'*************************************************************************************************************
IF test = 1 THEN                        'Print debug infor
PRINT "In analog output subroutine!"
END IF
offaddlb = strtadd + (8 + (2 * outport#))       'Determine the output register for low 8 bits
offaddhb = offaddlb + 1                 'Determine the output register for the next 4 bits
hb = INT(outvalu / 256)                 'Strip off the low 8 bits to get the high 4
lb = INT(outvalu - (256 * hb))          'Find the low for bits
OUT offaddlb, (lb)                      'Send the low byte
OUT offaddhb, (hb)                      'Send the high nibble
IF test = 1 THEN                        'Print debug info
PRINT outvalu, hb, lb
END IF
endsub

'*****************************************************************************
'****************************************************************************

SUB fuzspeed
'------------------------------------------------------------------------------------------------------------------
'Fuzzy Logic Controller for Motor Position
'
'*****************************************************************************
'               VARIABLES
'
'       PASSED IN
'
'       errval          :Difference between desired position and actual position
'       velcty          :Rate of change of the error value
'
'
'
'      INTERNAL
'
'       numrule         :Number of rules in fuzzy logic set
'       x               :Counter
'       ermemR( )       :Value of current errval relative to Rule ( )
'       vlmemR( )       :Value of current velcty relative to Rule ( )
'       outmemR( )      :Value of the output based on Rule ( ) if both inputs values = 1
'       contrbR( )      :Contribution of each rule to the final output
'
'       PASSED OUT
'
'       outvalu          :Value of output
'****************************************************************************************************************
'
IF test = 1 THEN
PRINT "In the fuzzy place"
END IF
numrule = 8                       'There are 8 rules for this system.
outmemR(1) = -2047              'This section describes the output for each
outmemR(2) = 2047               '  rule if the conditions of the rule were
outmemR(3) = -205               '  met exactly.
outmemR(4) = 205
outmemR(5) = -205
outmemR(6) = 205
outmemR(7) = -2047
outmemR(8) = 2047

ASSIGNE:                        'This section assigns the value for error
SELECT CASE errval              '  which is applicable to each rule, depending
	CASE IS < -4095         '  on the range error is in.
		PRINT "Malfunction in Position Error (-)"
		BEEP
		errval = -4095
		GOTO ASSIGN
	CASE -4095 TO -3276
		ermemR(1) = 0
		ermemR(2) = 1
		ermemR(3) = 0
		ermemR(4) = .3
		ermemR(5) = 0
		ermemR(6) = 0
		ermemR(7) = 0
		ermemR(8) = .3
	CASE -3275 TO -2475
		ermemR(1) = 0
		ermemR(2) = .7
		ermemR(3) = 0
		ermemR(4) = .6
		ermemR(5) = 0
		ermemR(6) = 0
		ermemR(7) = 0
		ermemR(8) = .6
	CASE -2474 TO -1638
		ermemR(1) = 0
		ermemR(2) = .4
		ermemR(3) = 0
		ermemR(4) = .9
		ermemR(5) = .1
		ermemR(6) = .1
		ermemR(7) = 0
		ermemR(8) = .9
	CASE -1637 TO -819
		ermemR(1) = 0
		ermemR(2) = .1
		ermemR(3) = 0
		ermemR(4) = .9
		ermemR(5) = .4
		ermemR(6) = .4
		ermemR(7) = 0
		ermemR(8) = .9
	CASE -818 TO -41
		ermemR(1) = 0
		ermemR(2) = 0
		ermemR(3) = 0
		ermemR(4) = .6
		ermemR(5) = .7
		ermemR(6) = .7
		ermemR(7) = 0
		ermemR(8) = .6
	CASE -40 TO 40
		ermemR(1) = 0
		ermemR(2) = 0
		ermemR(3) = .3
		ermemR(4) = .3
		ermemR(5) = 1
		ermemR(6) = 1
		ermemR(7) = .3
		ermemR(8) = .3
	CASE 41 TO 819
		ermemR(1) = 0
		ermemR(2) = 0
		ermemR(3) = .6
		ermemR(4) = 0
		ermemR(5) = .7
		ermemR(6) = .7
		ermemR(7) = .6
		ermemR(8) = 0
	CASE 820 TO 1638
		ermemR(1) = .1
		ermemR(2) = 0
		ermemR(3) = .9
		ermemR(4) = 0
		ermemR(5) = .4
		ermemR(6) = .4
		ermemR(7) = .9
		ermemR(8) = 0
	CASE 1639 TO 2475
		ermemR(1) = .4
		ermemR(2) = 0
		ermemR(3) = .9
		ermemR(4) = 0
		ermemR(5) = .1
		ermemR(6) = .1
		ermemR(7) = .9
		ermemR(8) = 0
	CASE 2476 TO 3276
		ermemR(1) = .7
		ermemR(2) = 0
		ermemR(3) = .6
		ermemR(4) = 0
		ermemR(5) = 0
		ermemR(6) = 0
		ermemR(7) = .6
		ermemR(8) = 0
	CASE 3277 TO 4095
		ermemR(1) = 1
		ermemR(2) = 0
		ermemR(3) = .3
		ermemR(4) = 0
		ermemR(5) = 0
		ermemR(6) = 0
		ermemR(7) = .3
		ermemR(8) = 0
	CASE ELSE
		PRINT "Malfunction in Position Error (+)"
		BEEP
		errval = 4095
		GOTO ASSIGNE
END SELECT
	     
       
ASSIGNV:                        'This section assigns the value for velocity
SELECT CASE velcty              '  which is applicable to each rule, depending
	CASE IS < -400          '  on the range the velocity is in.
		PRINT "Malfunction in Position Rate of Change (-)"
		BEEP
		velcty = -400
		GOTO ASSIGNV
	CASE -400 TO -267
		vlmemR(1) = 1
		vlmemR(2) = 1
		vlmemR(3) = 0
		vlmemR(4) = .467
		vlmemR(5) = 0
		vlmemR(6) = .467
		vlmemR(7) = .1
		vlmemR(8) = .1
	CASE -266 TO -133
		vlmemR(1) = 1
		vlmemR(2) = 1
		vlmemR(3) = .033
		vlmemR(4) = .8
		vlmemR(5) = .033
		vlmemR(6) = .8
		vlmemR(7) = .4
		vlmemR(8) = .4
	CASE -132 TO -1
		vlmemR(1) = 1
		vlmemR(2) = 1
		vlmemR(3) = .167
		vlmemR(4) = .7
		vlmemR(5) = .167
		vlmemR(6) = .7
		vlmemR(7) = .7
		vlmemR(8) = .7
	CASE 1 TO 133
		vlmemR(1) = 1
		vlmemR(2) = 1
		vlmemR(3) = .7
		vlmemR(4) = .167
		vlmemR(5) = .7
		vlmemR(6) = .167
		vlmemR(7) = .7
		vlmemR(8) = .7
	CASE 134 TO 267
		vlmemR(1) = 1
		vlmemR(2) = 1
		vlmemR(3) = .8
		vlmemR(4) = .033
		vlmemR(5) = .8
		vlmemR(6) = .033
		vlmemR(7) = .4
		vlmemR(8) = .4
	CASE 268 TO 400
		vlmemR(1) = 1
		vlmemR(2) = 1
		vlmemR(3) = .7
		vlmemR(4) = 0
		vlmemR(5) = .7
		vlmemR(6) = 0
		vlmemR(7) = .1
		vlmemR(8) = .1
	CASE ELSE
		velcty = 400
		PRINT "Malfunction in Position Rate of Change (+)"
		BEEP
		GOTO ASSIGNV
END SELECT
'
'Determine the input values for the rules where:
'               a and b --->    min(a,b)
'               a or b  --->    max(a,b)
'               not a   --->    1 - a
outvalu = 0                     'Initialize the output value
FOR x = 1 TO numrule            'Find the input value for each rule
	IF ermemR(x) > vlmemR(x) THEN
		Rinput(x) = vlmemR(x)
	ELSE                    '(Pick the minimum value from the two)
		Rinput(x) = ermemR(x)
	END IF                  'Now find the effect of each rule for
				'     the current inputs.
	contrb(x) = Rinput(x) * outmemR(x)
				'The total output is equal to the sum
				'     of the contributions from each Rule
	outval = outvalu + contrb(x)
NEXT x
END SUB