FUNCTION_BLOCK robot

VAR_INPUT
	dq : REAL;
	o : REAL;
	rd : REAL;
	v : REAL;
END_VAR

VAR_OUTPUT
	av : REAL;
	la : REAL;
END_VAR

FUZZIFY dq
	TERM H := TRIAN 0.0 2.0 2.0;
	TERM L := TRIAN 0.0 0.0 2.0;
END_FUZZIFY

FUZZIFY o
	TERM HL := TRIAN -45.0 -45.0 -22.5;
	TERM HR := TRIAN 22.5 45.0 45.0;
	TERM LL := TRIAN -45.0 -22.5 0.0;
	TERM LR := TRIAN 0.0 22.5 45.0;
	TERM Z := TRIAN -22.5 0.0 22.5;
END_FUZZIFY

FUZZIFY rd
	TERM H := TRIAN 1.0 2.0 3.0;
	TERM L := TRIAN 0.0 0.0 1.0;
	TERM M := TRIAN 0.0 1.0 2.0;
	TERM VH := TRIAN 2.0 3.0 3.0;
END_FUZZIFY

FUZZIFY v
	TERM H := TRIAN 0.0 1.0 1.0;
	TERM L := TRIAN 0.0 0.0 1.0;
END_FUZZIFY

DEFUZZIFY av
	TERM HL := TRIAN 0.5 0.75 1.0;
	TERM HR := TRIAN -1.0 -0.75 -0.5;
	TERM ML := TRIAN 0.25 0.5 0.75;
	TERM MR := TRIAN -0.75 -0.5 -0.25;
	TERM SL := TRIAN 0.0 0.25 0.5;
	TERM SR := TRIAN -0.5 -0.25 0.0;
	TERM VHL := TRIAN 0.75 1.0 1.0;
	TERM VHR := TRIAN -1.0 -1.0 -0.75;
	TERM Z := TRIAN -0.25 0.0 0.25;
	METHOD : COG;
	DEFAULT := 0.0;
	RANGE := (-1.0 .. 1.0);
END_DEFUZZIFY

DEFUZZIFY la
	TERM HA := TRIAN 0.5 0.75 1.0;
	TERM HB := TRIAN -1.0 -0.75 -0.5;
	TERM MA := TRIAN 0.25 0.5 0.75;
	TERM MB := TRIAN -0.75 -0.5 -0.25;
	TERM SA := TRIAN 0.0 0.25 0.5;
	TERM SB := TRIAN -0.5 -0.25 0.0;
	TERM VHA := TRIAN 0.75 1.0 1.0;
	TERM VHB := TRIAN -1.0 -1.0 -0.75;
	TERM Z := TRIAN -0.25 0.0 0.25;
	METHOD : COG;
	DEFAULT := 0.0;
	RANGE := (-1.0 .. 1.0);
END_DEFUZZIFY

RULEBLOCK rules
	ACT : MIN;
	ACCU : MAX;
	AND : MIN;
	RULE 01 : IF (((rd IS L) AND (dq IS L)) AND (o IS LL)) AND (v IS L) THEN la IS VHB , av IS VHR WITH 0.461;
	RULE 02 : IF (((rd IS L) AND (dq IS L)) AND (o IS LL)) AND (v IS H) THEN la IS VHB , av IS VHR WITH 0.4896;
	RULE 03 : IF (((rd IS L) AND (dq IS L)) AND (o IS Z)) AND (v IS L) THEN la IS Z , av IS MR WITH 0.6664;
	RULE 04 : IF (((rd IS L) AND (dq IS L)) AND (o IS Z)) AND (v IS H) THEN la IS HB , av IS SR WITH 0.5435;
	RULE 05 : IF (((rd IS L) AND (dq IS H)) AND (o IS LL)) AND (v IS L) THEN la IS MA , av IS HR WITH 0.7276;
	RULE 06 : IF (((rd IS L) AND (dq IS H)) AND (o IS Z)) AND (v IS L) THEN la IS MA , av IS HL WITH 0.4845;
	RULE 07 : IF (((rd IS L) AND (dq IS H)) AND (o IS Z)) AND (v IS H) THEN la IS HB , av IS ML WITH 0.5023;
	RULE 08 : IF (((rd IS L) AND (dq IS H)) AND (o IS LR)) AND (v IS H) THEN la IS VHB , av IS VHL WITH 0.7363;
	RULE 09 : IF (((rd IS L) AND (dq IS H)) AND (o IS HR)) AND (v IS L) THEN la IS VHB , av IS VHL WITH 0.9441;
	RULE 10 : IF (((rd IS M) AND (dq IS L)) AND (o IS Z)) AND (v IS H) THEN la IS SA , av IS HR WITH 0.3402;
	RULE 11 : IF (((rd IS M) AND (dq IS L)) AND (o IS LR)) AND (v IS H) THEN la IS Z , av IS VHL WITH 0.4244;
	RULE 12 : IF (((rd IS M) AND (dq IS L)) AND (o IS HR)) AND (v IS L) THEN la IS SA , av IS HL WITH 0.5472;
	RULE 13 : IF (((rd IS M) AND (dq IS L)) AND (o IS HR)) AND (v IS H) THEN la IS MB , av IS VHL WITH 0.4369;
	RULE 14 : IF (((rd IS M) AND (dq IS H)) AND (o IS HL)) AND (v IS L) THEN la IS Z , av IS VHR WITH 0.177;
	RULE 15 : IF (((rd IS M) AND (dq IS H)) AND (o IS HL)) AND (v IS H) THEN la IS VHB , av IS VHR WITH 0.4526;
	RULE 16 : IF (((rd IS M) AND (dq IS H)) AND (o IS LL)) AND (v IS H) THEN la IS SA , av IS VHR WITH 0.2548;
	RULE 17 : IF (((rd IS M) AND (dq IS H)) AND (o IS Z)) AND (v IS L) THEN la IS HA , av IS Z WITH 0.2084;
	RULE 18 : IF (((rd IS M) AND (dq IS H)) AND (o IS LR)) AND (v IS L) THEN la IS HA , av IS VHL WITH 0.6242;
	RULE 19 : IF (((rd IS M) AND (dq IS H)) AND (o IS LR)) AND (v IS H) THEN la IS SA , av IS VHL WITH 0.3779;
	RULE 20 : IF (((rd IS M) AND (dq IS H)) AND (o IS HR)) AND (v IS L) THEN la IS Z , av IS VHL WITH 0.6931;
	RULE 21 : IF (((rd IS M) AND (dq IS H)) AND (o IS HR)) AND (v IS H) THEN la IS VHB , av IS VHL WITH 0.758;
	RULE 22 : IF (((rd IS H) AND (dq IS L)) AND (o IS Z)) AND (v IS L) THEN la IS HA , av IS VHR WITH 0.5758;
	RULE 23 : IF (((rd IS H) AND (dq IS L)) AND (o IS LR)) AND (v IS H) THEN la IS SA , av IS MR WITH 0.2513;
	RULE 24 : IF (((rd IS H) AND (dq IS L)) AND (o IS HR)) AND (v IS L) THEN la IS HA , av IS VHL WITH 0.5471;
	RULE 25 : IF (((rd IS H) AND (dq IS L)) AND (o IS HR)) AND (v IS H) THEN la IS SA , av IS HL WITH 0.5595;
	RULE 26 : IF (((rd IS H) AND (dq IS H)) AND (o IS HL)) AND (v IS L) THEN la IS VHB , av IS VHR WITH 0.9999;
	RULE 27 : IF (((rd IS H) AND (dq IS H)) AND (o IS HL)) AND (v IS H) THEN la IS VHB , av IS VHR WITH 0.9563;
	RULE 28 : IF (((rd IS H) AND (dq IS H)) AND (o IS LL)) AND (v IS L) THEN la IS HA , av IS VHR WITH 0.9506;
	RULE 29 : IF (((rd IS H) AND (dq IS H)) AND (o IS Z)) AND (v IS L) THEN la IS HA , av IS VHR WITH 0.4529;
	RULE 30 : IF (((rd IS H) AND (dq IS H)) AND (o IS Z)) AND (v IS H) THEN la IS SA , av IS VHR WITH 0.221;
	RULE 31 : IF (((rd IS H) AND (dq IS H)) AND (o IS LR)) AND (v IS L) THEN la IS HA , av IS MR WITH 0.3612;
	RULE 32 : IF (((rd IS H) AND (dq IS H)) AND (o IS LR)) AND (v IS H) THEN la IS SA , av IS MR WITH 0.2122;
	RULE 33 : IF (((rd IS H) AND (dq IS H)) AND (o IS HR)) AND (v IS L) THEN la IS HA , av IS HL WITH 0.7878;
	RULE 34 : IF (((rd IS H) AND (dq IS H)) AND (o IS HR)) AND (v IS H) THEN la IS SA , av IS VHL WITH 0.3859;
	RULE 35 : IF (((rd IS VH) AND (dq IS L)) AND (o IS LR)) AND (v IS L) THEN la IS HA , av IS VHR WITH 0.553;
	RULE 36 : IF (((rd IS VH) AND (dq IS L)) AND (o IS HR)) AND (v IS L) THEN la IS HA , av IS HR WITH 0.4223;
	RULE 37 : IF (((rd IS VH) AND (dq IS L)) AND (o IS HR)) AND (v IS H) THEN la IS SA , av IS HR WITH 0.3854;
	RULE 38 : IF (((rd IS VH) AND (dq IS H)) AND (o IS LL)) AND (v IS L) THEN la IS HA , av IS VHR WITH 0.0936;
	RULE 39 : IF (((rd IS VH) AND (dq IS H)) AND (o IS LR)) AND (v IS L) THEN la IS HA , av IS VHR WITH 0.7325;
	RULE 40 : IF (((rd IS VH) AND (dq IS H)) AND (o IS LR)) AND (v IS H) THEN la IS SA , av IS VHR WITH 0.5631;
	RULE 41 : IF (((rd IS VH) AND (dq IS H)) AND (o IS HR)) AND (v IS L) THEN la IS HA , av IS HR WITH 0.5146;
END_RULEBLOCK

END_FUNCTION_BLOCK