/* Robot controller. References: "Learning Weighted LinguIStic Rules to Control an Autonomous Robot" , M. Mucientes, R. Alcala , J. Alcala-Fdez, J. Casillas Pablo Cingolani pcingola@users.sourceforge.net */ FUNCTION_BLOCK robot VAR_INPUT rd : REAL; // Right dIStance dq : REAL; // DIStance quotient o : REAL; // Orientation. Note: I cannot use 'or' (like the reference paper) because it IS a reserved word in FCL v : REAL; // Velocity END_VAR VAR_OUTPUT la : REAL; // Linear acceleration av : REAL; // Angular velocity END_VAR FUZZIFY rd TERM L := TRIAN 0 0 1; TERM M := TRIAN 0 1 2; TERM H := TRIAN 1 2 3; TERM VH := TRIAN 2 3 3; END_FUZZIFY FUZZIFY dq TERM L := TRIAN 0 0 2; TERM H := TRIAN 0 2 2; END_FUZZIFY FUZZIFY o TERM HL := TRIAN -45 -45 -22.5; TERM LL := TRIAN -45 -22.5 0; TERM Z := TRIAN -22.5 0 22.5; TERM LR := TRIAN 0 22.5 45; TERM HR := TRIAN 22.5 45 45; END_FUZZIFY FUZZIFY v TERM L := TRIAN 0 0 1; TERM H := TRIAN 0 1 1; END_FUZZIFY DEFUZZIFY la TERM VHB := TRIAN -1 -1 -0.75; TERM HB := TRIAN -1 -0.75 -0.5; TERM MB := TRIAN -0.75 -0.5 -0.25; TERM SB := TRIAN -0.5 -0.25 0; TERM Z := TRIAN -0.25 0 0.25; TERM SA := TRIAN 0 0.25 0.5; TERM MA := TRIAN 0.25 0.5 0.75; TERM HA := TRIAN 0.5 0.75 1; TERM VHA := TRIAN 0.75 1 1; METHOD : COG; DEFAULT := 0; END_DEFUZZIFY DEFUZZIFY av TERM VHR := TRIAN -1 -1 -0.75; TERM HR := TRIAN -1 -0.75 -0.5; TERM MR := TRIAN -0.75 -0.5 -0.25; TERM SR := TRIAN -0.5 -0.25 0; TERM Z := TRIAN -0.25 0 0.25; TERM SL := TRIAN 0 0.25 0.5; TERM ML := TRIAN 0.25 0.5 0.75; TERM HL := TRIAN 0.5 0.75 1; TERM VHL := TRIAN 0.75 1 1; METHOD : COG; DEFAULT := 0; END_DEFUZZIFY RULEBLOCK rules AND : MIN; // Use 'min' for 'AND' (also implicit use 'max' for 'or' to fulfill DeMorgan's Law) ACT : MIN; // Use 'min' activation method ACCU : MAX; // Use 'max' accumulation method 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.4610; 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.1770; 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.7580; 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.2210; 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.5530; 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