119 lines
5.8 KiB
Plaintext
119 lines
5.8 KiB
Plaintext
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
|
|
|