clearvars % Effacer les variables
clf; % effacer la figure

dt = 0.2; % increment en temps
V     = [ones(1,10) 1.5*ones(1,10) 0.7*ones(1,10)];
Omega = [zeros(1,10) 1*ones(1,10) -1*ones(1,10)];
Time  = (0:(size(V,2)-1))*dt;

% Initialisation
Theta(1) = 0; x(1) = 0; y(1) = 0;

for t = 2:size(V,2)
    x(t) = x(t-1) + V(t)*cos(Theta(t-1))*dt; % Intégration x
    y(t) = y(t-1) + V(t)*sin(Theta(t-1))*dt; % Intégration y
    Theta(t) = Theta(t-1) + Omega(t)*dt; % Intégration angle
end


subplot(2,2,1);
plot(Time,Omega,'LineWidth',2);
xlabel('Temps (s)','FontSize',18);
ylabel({'Vitesse' 'Angulaire \omega (rad/s)'},'FontSize',18);
set(gca,'FontSize',16); % Changer la taille de la police du graphique
ylim([-1.1 1.1]);

subplot(2,2,3);
plot(Time,V,'LineWidth',2);
xlabel('Temps (s)','FontSize',18);
ylabel({'Vitesse' 'V (m/s)'},'FontSize',18);
set(gca,'FontSize',16); % Changer la taille de la police du graphique
ylim([0 1.7]);

subplot(2,2,[2 4]);
% Point de départ
%h(1) = plot(x,y,'go','MarkerSize',6,'MarkerFaceColor','g','MarkerEdgeColor','g');
hlegend (1) = plot(x,y,'o','MarkerSize',8);
hold on;
hl = 0.08;
for index = 1:size(V,2)
    hlegend (2) = line([x(index) x(index)+ hl*cos(Theta(index))], ...
         [y(index) y(index)+ hl*sin(Theta(index))], 'Color', 'b','LineWidth',2);
end
set(gca,'FontSize',18); % Changer la taille de la police du graphique
xlabel('x (m)','FontSize',18);
ylabel('y (m)','FontSize',18);
axis equal % pour avoir échelle en x pareil en y
ylim([-0.2 3.2])

% print -dpng ExempleDiffDriveSansBruit.png
