89 lines
2.0 KiB
Matlab
89 lines
2.0 KiB
Matlab
%% Car - Simulation
|
||
%
|
||
% Assignment 2 in Fuzzy systems
|
||
%
|
||
% author:
|
||
% Christos Choutouridis ΑΕΜ 8997
|
||
% cchoutou@ece.auth.gr
|
||
%
|
||
clear; clc; close all;
|
||
|
||
global Speed_mag;
|
||
global Ts;
|
||
|
||
|
||
% Configuration
|
||
% ----------------------
|
||
Speed_mag = 0.05;
|
||
Ts = 1;
|
||
|
||
% Givents
|
||
% ----------------------
|
||
corners = [5 1 ; 6 2 ; 7 3];
|
||
x_init = 3.8;
|
||
y_init = 0.5;
|
||
x_final = 10;
|
||
y_final = 3.2;
|
||
theta_init = [-45 0 45];
|
||
|
||
|
||
% Saturation between min: m and max: M
|
||
sat = @(x, m, M) max(m, min(M, x));
|
||
|
||
% Load control
|
||
flc = readfis('CarFLC');
|
||
|
||
t = 0:Ts:250;
|
||
for i = 1:length(theta_init)
|
||
x = x_init;
|
||
y = y_init;
|
||
theta = theta_init(i);
|
||
|
||
X = zeros(size(t));
|
||
Y = zeros(size(t));
|
||
|
||
End=length(t);
|
||
for k=1:End
|
||
[dh, dv] = wallDistance(corners, x, y);
|
||
if isfinite(dh) && ~isfinite(dv) && (dh <=0 || dv <= 0)
|
||
End = k-1;
|
||
fprintf('Car crashed!! - Stop simulation');
|
||
break;
|
||
end
|
||
|
||
dtheta = evalfis(flc, [sat(dh, 0, 1) sat(dv, 0, 1) theta]);
|
||
[x, y, theta] = moveCar(x, y, theta, dtheta);
|
||
X(k) = x; Y(k) = y;
|
||
|
||
if x >= x_final
|
||
End = k-1;
|
||
break;
|
||
end
|
||
end
|
||
|
||
figure(i); clf; hold on; grid on; axis equal;
|
||
xlim([2 11]); ylim([-1 4]);
|
||
xlabel('x [m]'); ylabel('y [m]');
|
||
title( ...
|
||
"Car movement $(\theta_0 =" + theta_init(i)+ "^{\circ})$", ...
|
||
'Interpreter','latex', ...
|
||
'FontSize', 16);
|
||
|
||
% Walls
|
||
fill([5 6 6 5],[0 0 1 1],[0.5 0.5 0.5],'FaceAlpha',0.4,'EdgeColor','none');
|
||
fill([6 7 7 6],[0 0 2 2],[0.5 0.5 0.5],'FaceAlpha',0.4,'EdgeColor','none');
|
||
fill([7 10 10 7],[0 0 3 3],[0.5 0.5 0.5],'FaceAlpha',0.4,'EdgeColor','none');
|
||
|
||
plot(X(1:End), Y(1:End), '-', 'LineWidth', 1.5);
|
||
|
||
hold on;
|
||
plot(X(1), Y(1), 'go', 'MarkerFaceColor', 'g'); % begin
|
||
plot(X(End), Y(End), 'ro', 'MarkerFaceColor', 'r'); % end
|
||
hold off;
|
||
|
||
filename = sprintf('CarMovement_theta%d.png', theta_init(i));
|
||
saveas(gcf, filename);
|
||
end
|
||
|
||
|