-
Notifications
You must be signed in to change notification settings - Fork 0
/
driver.fcl
91 lines (78 loc) · 2.51 KB
/
driver.fcl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
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