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