FUNCTION_BLOCK driver

VAR_INPUT
  // angles {-90,-75,-60,-45,-30,-20,-15,-10,-5,0,5,10,15,20,30,45,60,75,90}
  track0: REAL;
  track1: REAL;
  track2: REAL;
  track3: REAL;
  track4: REAL;
  track5: REAL;
  track6: REAL;
  track7: REAL;
  track8: REAL;
  // angle 0
  track9: REAL;
  track10: REAL;
  track11: REAL;
  track12: REAL;
  track13: REAL;
  track14: REAL;
  track15: REAL;
  track16: REAL;
  track17: REAL;
  track18: REAL;

  trackPos: REAL;
  speed: REAL;

  opponent0: REAL;
  opponent1: REAL;
  opponent2: REAL;
  opponent3: REAL;
  opponent4: REAL;
  opponent5: REAL;
  opponent6: REAL;
  opponent7: REAL;
  opponent8: REAL;
  opponent9: REAL;
  opponent10: REAL;
  opponent11: REAL;
  opponent12: REAL;
  opponent13: REAL;
  opponent14: REAL;
  opponent15: REAL;
  opponent16: REAL;
  opponent17: REAL;
  opponent18: REAL;
  opponent19: REAL;
  opponent20: REAL;
  opponent21: REAL;
  opponent22: REAL;
  opponent23: REAL;
  opponent24: REAL;
  opponent25: REAL;
  opponent26: REAL;
  opponent27: REAL;
  opponent28: REAL;
  opponent29: REAL;
  opponent30: REAL;
  opponent31: REAL;
  opponent32: REAL;
  opponent33: REAL;
  opponent34: REAL;
  opponent35: REAL;

  angle: REAL;
END_VAR

VAR_OUTPUT
  steering: REAL;
  accelerate: REAL;
  brake: REAL;
END_VAR

FUZZIFY trackPos
  TERM left := (0.1, 0) (0.45, 1) (9,1);
  TERM center := (-0.75, 0) (-0.4, 1) (0.4, 1) (0.75, 0);
  TERM right := (-9, 1) (-0.45, 1) (-0.1, 0);
END_FUZZIFY

FUZZIFY angle
  TERM left := (-1.875, 1) (-0.63893, 1) (0, 0);
  TERM center := (-0.28823, 0) (-0.1, 1) (0.28823, 0);
  TERM right := (0, 0) (0.51893, 1) (1.875, 1);
END_FUZZIFY


FUZZIFY track9
  TERM outside := (-1, 1) (-0.001, 1) (0, 0);
  TERM near := (-0.001, 0) (0, 1) (20, 1) (25, 0);
  TERM mid := (15, 0) (35, 1) (55, 0);
  TERM far := (51, 0) (80, 1) (200, 1);
END_FUZZIFY

FUZZIFY speed
  TERM f_slow :=  (0, 1) (25, 1) (40, 0);
  TERM f_mid :=  (37, 0) (70, 1) (88, 0);
  TERM f_fast :=  (70, 0) (90, 1) (120, 1) (140, 0);
  TERM f_very_fast :=  (100, 0) (150, 1) (300, 1);
END_FUZZIFY

DEFUZZIFY steering
  TERM turn_left :=  (0.005, 0) (0.25, 1) (1, 1);
  TERM stay_center :=  (-0.1, 0) (0, 1) (0.1, 0);
  TERM turn_right :=  (-1, 1) (-0.1, 1) (0, 0);
  METHOD : COG;
  DEFAULT := 0.000;
END_DEFUZZIFY

DEFUZZIFY accelerate
  TERM nogas :=  (0, 1) (0.1, 1) (0.2, 0);
  TERM medgas :=  (0.1, 0) (0.34, 1) (0.4, 0);
  TERM fullgas :=  (0.35, 0) (0.4, 1) (1, 1);
  METHOD : COG;
  DEFAULT := 1.000;
END_DEFUZZIFY

DEFUZZIFY brake
  TERM nobrake :=  (0, 1) (0.035, 0) ;
  TERM fullbrake :=  (0.035, 0) (0.09, 1) (1, 1);
  METHOD : COG;
  DEFAULT := 0.000;
END_DEFUZZIFY

RULEBLOCK No1
  AND : MIN;
  ACT : MIN;
  ACCU : MAX;

  RULE 1 : IF trackPos IS left THEN steering IS turn_right;
  RULE 2 : IF trackPos IS center THEN steering IS stay_center;
  RULE 3 : IF trackPos IS right THEN steering IS turn_left;
  RULE 4 : IF angle IS left THEN steering IS turn_right;
  RULE 5 : IF angle IS center THEN steering IS stay_center;
  RULE 6 : IF angle IS right THEN steering IS turn_left;

  RULE 7 : IF track9 IS near AND speed IS f_fast THEN accelerate IS nogas;
  RULE 8 : IF track9 IS far AND speed IS f_slow THEN accelerate IS fullgas, brake IS nobrake;
  RULE 9 : IF track9 IS mid AND speed IS f_slow THEN accelerate IS fullgas, brake IS nobrake;
  RULE 10 : IF track9 IS outside THEN accelerate is medgas, brake IS nobrake;

  RULE 11 : IF track9 IS near AND speed IS f_very_fast THEN accelerate IS nogas, brake IS fullbrake;
  RULE 12 : IF track9 IS mid AND speed IS f_mid THEN accelerate IS fullgas, brake IS nobrake;
END_RULEBLOCK

END_FUNCTION_BLOCK