91 lines
2.5 KiB
Plaintext
91 lines
2.5 KiB
Plaintext
FUNCTION_BLOCK driver
|
|
|
|
VAR_INPUT
|
|
trackPos: REAL;
|
|
angle: REAL;
|
|
track9: REAL;
|
|
speedX: REAL;
|
|
END_VAR
|
|
|
|
VAR_OUTPUT
|
|
steer: REAL;
|
|
accel: REAL;
|
|
brake: REAL;
|
|
END_VAR
|
|
|
|
FUZZIFY trackPos
|
|
RANGE := (-9.000 .. 9.000);
|
|
TERM left := Trapezoid 0.300 0.600 9.000 9.000;
|
|
TERM center := Trapezoid -0.600 -0.300 0.300 0.600;
|
|
TERM right := Trapezoid -9.000 -9.000 -0.600 -0.300;
|
|
END_FUZZIFY
|
|
|
|
FUZZIFY angle
|
|
RANGE := (-3.150 .. 3.150);
|
|
TERM left := Trapezoid -3.150 -3.150 -0.900 -0.000;
|
|
TERM center := Trapezoid -0.900 -0.000 0.000 0.900;
|
|
TERM right := Trapezoid 0.000 0.900 3.150 3.150;
|
|
END_FUZZIFY
|
|
|
|
FUZZIFY track9
|
|
RANGE := (-1.000 .. 200.000);
|
|
TERM outside := Trapezoid -1.000 -1.000 0.000 0.000;
|
|
TERM near := Trapezoid 0.000 0.000 30.000 60.000;
|
|
TERM far := Trapezoid 30.000 60.000 200.00 200.000;
|
|
END_FUZZIFY
|
|
|
|
FUZZIFY speedX
|
|
RANGE := (0.000 .. 300.000);
|
|
TERM f_slow := Trapezoid 0.000 0.000 15.000 30.000;
|
|
TERM f_fast := Trapezoid 15.000 30.000 100.00 130.000;
|
|
TERM f_very_fast := Trapezoid 100.000 130.000 300.00 300.000;
|
|
END_FUZZIFY
|
|
|
|
DEFUZZIFY steer
|
|
RANGE := (-1.000 .. 1.000);
|
|
TERM turn_left := Trapezoid 0.000 0.100 1.000 1.000;
|
|
TERM stay_center := Triangle -0.100 0.000 0.100;
|
|
TERM turn_right := Trapezoid -1.000 -1.000 -0.100 0.000;
|
|
METHOD : COG;
|
|
ACCU : MAX;
|
|
DEFAULT := 0.000;
|
|
END_DEFUZZIFY
|
|
|
|
DEFUZZIFY accel
|
|
RANGE := (0.000 .. 1.000);
|
|
TERM nogas := Trapezoid 0.000 0.000 0.300 0.500;
|
|
TERM medgas := Triangle 0.100 0.200 0.300;
|
|
TERM fullgas := Trapezoid 0.300 0.400 1.000 1.000;
|
|
METHOD : COG;
|
|
ACCU : MAX;
|
|
DEFAULT := 1.000;
|
|
END_DEFUZZIFY
|
|
|
|
DEFUZZIFY brake
|
|
RANGE := (0.000 .. 1.000);
|
|
TERM nobrake := Triangle 0.000 0.000 0.035;
|
|
TERM fullbrake := Trapezoid 0.035 0.100 1.000 1.000;
|
|
METHOD : COG;
|
|
ACCU : MAX;
|
|
DEFAULT := 0.000;
|
|
END_DEFUZZIFY
|
|
|
|
RULEBLOCK
|
|
AND : MIN;
|
|
OR : MAX;
|
|
ACT : MIN;
|
|
RULE 1 : if trackPos is left then steer is turn_right;
|
|
RULE 2 : if trackPos is center then steer is stay_center;
|
|
RULE 3 : if trackPos is right then steer is turn_left;
|
|
RULE 4 : if angle is left then steer is turn_right;
|
|
RULE 5 : if angle is center then steer is stay_center;
|
|
RULE 6 : if angle is right then steer is turn_left;
|
|
|
|
RULE 7 : if track9 is near and speedX is f_fast then accel is nogas;
|
|
RULE 8 : if track9 is far and speedX is f_slow then accel is fullgas and brake is nobrake;
|
|
RULE 9 : if track9 is outside then accel is medgas and brake is nobrake;
|
|
|
|
RULE 10 : if track9 is near and speedX is f_very_fast then accel is nogas and brake is fullbrake;
|
|
END_RULEBLOCK
|
|
|
|
END_FUNCTION_BLOCK |