% Programa em MATLAB com animação em tempo real de dois referenciais
clear all;
close all;
% Parâmetros da trajetória
r = 4; % Raio
length_straight = 15; % Comprimento da aresta
% Coordenadas dos semicírculos
thetaa = linspace(0, pi, 100); % Semicírculo superior
xa = r * cos(thetaa);
ya = r * sin(thetaa) + length_straight;
thetab = linspace(pi, 2*pi, 100); % Semicírculo inferior
xb = r * cos(thetab);
yb = r * sin(thetab) - length_straight;
% Coordenadas das partes retas
xesq = [-r, -r];
yesq = [-length_straight, length_straight];
xdir = [r, r];
ydir = [-length_straight, length_straight];
% Concatenar a trajetória em um único vetor (fechado)
x_traj = [xesq, xa, xdir(end:-1:1), xb];
y_traj = [yesq, ya, ydir(end:-1:1), yb];
% Calcular comprimento total da trajetória
total_length = 0;
seg_lengths = zeros(1, length(x_traj)-1);
for i = 1:length(x_traj)-1
seg_lengths(i) = sqrt((x_traj(i+1) - x_traj(i))^2 + (y_traj(i+1) -
y_traj(i))^2);
total_length = total_length + seg_lengths(i);
end
% Parâmetros da simulação
dt = 0.01; % Passo de tempo
t_total = 20; % Tempo total de simulação
v = total_length / 10; % Velocidade (uma volta em 10 segundos)
k_p = 0.1; % Ganho proporcional
epsilon = 1; % Limiar para iniciar o seguimento
step_size = 0.1; % Tamanho do passo na aproximação
% Inicialização dos referenciais
x_ref1 = 0; % Referencial 1 (aproximação) começa em [0, 0]
y_ref1 = 0;
x_ref2 = x_traj(1); % Referencial 2 (seguimento) começa na trajetória
y_ref2 = y_traj(1);
% Configurar a figura
figure('Position', [100, 100, 800, 600]);
subplot(2, 1, 1);
h_traj = plot(x_traj, y_traj, 'b-', 'LineWidth', 2); hold on;
h_ref1 = plot(x_ref1, y_ref1, 'ro', 'MarkerSize', 8, 'LineWidth', 2); % Referencial
1 (vermelho)
h_ref2 = plot(x_ref2, y_ref2, 'go', 'MarkerSize', 8, 'LineWidth', 2); % Referencial
2 (verde)
title('Movimento dos Referenciais');
xlabel('x'); ylabel('y');
legend('Trajetória', 'Referencial 1 (Aproximação)', 'Referencial 2 (Seguimento)');
grid on; axis equal;
axis([-10 10 -20 20]);
% Gráficos de erro
subplot(2, 1, 2);
h_err_x = plot(0, 0, 'r-', 'LineWidth', 1.5); hold on;
h_err_y = plot(0, 0, 'b-', 'LineWidth', 1.5);
title('Erros de Posição do Referencial 2');
xlabel('Tempo (s)'); ylabel('Erro (unidades)');
legend('Erro em x', 'Erro em y');
grid on; axis([0 t_total -5 5]);
% Variáveis para animação
t = 0;
following = false; % Para Referencial 1
lambda = 0; % Para Referencial 2
t_hist = [];
e_x_hist = [];
e_y_hist = [];
% Simulação em tempo real
tic; % Iniciar cronômetro
while t < t_total
% --- Referencial 1 (Aproximação) ---
min_dist = inf;
idx_closest = 1;
for j = 1:length(x_traj)
dist = sqrt((x_traj(j) - x_ref1)^2 + (y_traj(j) - y_ref1)^2);
if dist < min_dist
min_dist = dist;
idx_closest = j;
end
end
if ~following && min_dist > epsilon
% Mover em direção ao ponto mais próximo
dx = x_traj(idx_closest) - x_ref1;
dy = y_traj(idx_closest) - y_ref1;
norm = sqrt(dx^2 + dy^2);
if norm > 0
x_ref1 = x_ref1 + step_size * dx / norm;
y_ref1 = y_ref1 + step_size * dy / norm;
end
else
following = true; % Começa a seguir, mas mantém posição para simplicidade
end
% --- Referencial 2 (Seguimento) ---
lambda = mod(v * t, total_length);
cum_length = 0;
x_des = x_traj(1);
y_des = y_traj(1);
for j = 1:length(seg_lengths)
if cum_length + seg_lengths(j) >= lambda
frac = (lambda - cum_length) / seg_lengths(j);
x_des = x_traj(j) + frac * (x_traj(j+1) - x_traj(j));
y_des = y_traj(j) + frac * (y_traj(j+1) - y_traj(j));
break;
end
cum_length = cum_length + seg_lengths(j);
end
% Calcular erros
e_x = x_des - x_ref2;
e_y = y_des - y_ref2;
% Controlador proporcional
v_x = k_p * e_x;
v_y = k_p * e_y;
% Atualizar posição do Referencial 2
x_ref2 = x_ref2 + v_x * dt;
y_ref2 = y_ref2 + v_y * dt;
% Atualizar gráficos
set(h_ref1, 'XData', x_ref1, 'YData', y_ref1);
set(h_ref2, 'XData', x_ref2, 'YData', y_ref2);
% Armazenar histórico de erros
t_hist = [t_hist, t];
e_x_hist = [e_x_hist, e_x];
e_y_hist = [e_y_hist, e_y];
set(h_err_x, 'XData', t_hist, 'YData', e_x_hist);
set(h_err_y, 'XData', t_hist, 'YData', e_y_hist);
% Atualizar figura e controlar tempo
drawnow;
elapsed = toc;
if elapsed < t
pause(t - elapsed); % Sincronizar com tempo real
end
t = t + dt;
end