-
Notifications
You must be signed in to change notification settings - Fork 2
/
truck_reversing_fuzzy_controller.m
60 lines (53 loc) · 1.91 KB
/
truck_reversing_fuzzy_controller.m
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
function [status, x, y, phi_radius, steps] = truck_reversing_fuzzy_controller(controlModel, x, y, phi, alpha, image)
iteration = 150;
error = 0.05;
if isempty(alpha) == 1
alpha = 1;
end
if image == 1
patch([0; 0; 100; 100], [0; 100; 100; 0], 'w', 'linewidth', 2);
axis([0 100 0 100]);
figure(1);
hold on;
midLength = 1.6;
buttonLength = 0.1;
end
for steps = 0 : iteration;
if steps ~= 0
theta = evalfis([x, phi], controlModel);
phi = phi + theta;
phi_radius = (phi*pi) / 180;
x_former = x;
y_former = y;
x = x + alpha*cos(phi_radius);
y = y + alpha*sin(phi_radius);
if x < 0 || x > 100
x = x_former;
break;
end
if y < 0 || y > 100
y = y_former;
break;
end
if image == 1
peak_coor = [x - midLength*cos(phi_radius), y - midLength*sin(phi_radius)];
side_angle = (pi / 2) - phi_radius;
side_coor_1 = [x + buttonLength*cos(side_angle), y - buttonLength*sin(side_angle)];
side_coor_2 = [x - buttonLength*cos(side_angle), y + buttonLength*sin(side_angle)];
x_tri = [peak_coor(1, 1); side_coor_1(1, 1); side_coor_2(1, 1)];
y_tri = [peak_coor(1, 2); side_coor_1(1, 2); side_coor_2(1, 2)];
%plot(x, y, '--rs', 'LineWidth', 1, 'MarkerEdgeColor', 'k', 'MarkerFaceColor', 'g', 'MarkerSize', 5);
gplot(rand(3), [x_tri, y_tri], 'r');
%patch(x_tri, y_tri, 'r');
end
end
end
if image == 1
hold off;
end
if phi <= pi*(1 + error) / 2 && phi >= pi*(1 - error) / 2
status = 1;
else
status = 0;
end
end