clc; clear all;close all;
%% zadání geometrie
R=0.0385; % [m] poloměr mecanum koleček
W=0.102; % [m] 0.5*rozchod vozítka
H=0.0475; % [m] 0.5*rozvor vozítka
G=[0 0 0;-0.1475 -0.105 pi/4];  %umístění podvozků
%% zadání parametrů simulace
dt=0.05;
tf=125;
%% počáteční podmínky
x0=1; % počáteční poloha v x
y0=-1; % počáteční poloha v y
phi0=0; % počáteční natočení
%% výpočty
t=0:dt:tf;
n=1/dt;
[Wmax,Hmax,J,J_plus,MSC_S,PSI,D,K]=funkce_G(R,W,H,G);
Wv=1.5*Wmax; % [m] 0.5*šířka vozidla
Hv=1.25*Hmax; % [m] 0.5*délka vozidla
Rohy_S=[  Wv -Wv -Wv Wv;
        Hv  Hv  -Hv -Hv];
Voziky=[  1.25*W -1.25*W -1.25*W 1.25*W;
        1.5*H  1.5*H  -1.5*H -1.5*H];
KSI_10=[x0;y0;phi0]; % matice počáteční polohy 
MSC0=[cos(phi0)    -sin(phi0) 0;
        sin(phi0)    cos(phi0)  0;
        0               0       1];
P0_S=zeros(size(G,1)*3,1);
P0=zeros(size(G,1)*3,1);
for i=1:size(G,1)
    P0_S(1+(3*(i-1)):i*3,1)=MSC0*transpose(G(i,:));
    P0(1+(3*(i-1)):i*3,1)=KSI_10+P0_S(1+(3*(i-1)):i*3,1);
end
KSI_1=[KSI_10,zeros(3,length(t)-1)]; % matice polohy v základním rámu
KSI_dot_1=zeros(3,length(t)); %  matice rychlostí v základním rámu
KSI_dot_S=zeros(3,length(t)); % matice rychlostí vozidla v rámu vozidla
PHI_dot=zeros(4*size(G,1),length(t));
V_S=zeros(3*size(G,1),length(t));
V=zeros(3*size(G,1),length(t));
P=[P0,zeros(size(G,1)*3,length(t)-1)];
%% návrh trajektorie
k=0;
for i=1:length(t)
phi=KSI_1(3,i); % aktuální natočení v základním rámu 
J_s=[cos(phi), -sin(phi), 0 %jakobián vozidlo => zálkadní rám
    sin(phi), cos(phi), 0
    0           0       1];
if t(i)<=5
xs_dot=0; %rychlost vozidla v "x" v rámu vozidla
ys_dot=0; %rychlost vozidla v "y" v rámu vozidla
omegas_dot=0; % úhlová rychlost vozidla v rámu vozidla
elseif t(i)>5 && t(i)<=25
xs_dot=0; %rychlost vozidla v "x" v rámu vozidla
ys_dot=0.1; %rychlost vozidla v "y" v rámu vozidla
omegas_dot=0; % úhlová rychlost vozidla v rámu vozidla
elseif t(i)>25 && t(i)<=65
xs_dot=-0.05; %rychlost vozidla v "x" v rámu vozidla
ys_dot=-0.05; %rychlost vozidla v "y" v rámu vozidla
omegas_dot=0; % úhlová rychlost vozidla v rámu vozidla
elseif t(i)>65 && t(i)<=85
xs_dot=0; %rychlost vozidla v "x" v rámu vozidla
ys_dot=0.0785; %rychlost vozidla v "y" v rámu vozidla
omegas_dot=-4.5/180*pi; % úhlová rychlost vozidla v rámu vozidla
elseif t(i)>85 && t(i)<=105
xs_dot=-0.0785; %rychlost vozidla v "x" v rámu vozidla
ys_dot=0; %rychlost vozidla v "y" v rámu vozidla
omegas_dot=4.5/180*pi; % úhlová rychlost vozidla v rámu vozidla
else 
xs_dot=0; %rychlost vozidla v "x" v rámu vozidla
ys_dot=0; %rychlost vozidla v "y" v rámu vozidla
omegas_dot=9/180*pi; % úhlová rychlost vozidla v rámu vozidla   
 end
KSI_dot_S(:,i)=[xs_dot;ys_dot;omegas_dot];
KSI_dot_1(:,i)=J_s*KSI_dot_S(:,i);
if i<=length(t)-1
KSI_1(:,i+1)=KSI_1(:,i)+dt*KSI_dot_1(:,i);
for j=1:size(G,1)
    PHI_dot(1+(4*(j-1)):j*4,i)=J*K(1+(3*(j-1)):j*3,:)*KSI_dot_S(:,i);
    V_S(1+(3*(j-1)):j*3,i)=MSC_S(1+(3*(j-1)):j*3,:)*J_plus*PHI_dot(1+(4*(j-1)):j*4,i);
    V(1+(3*(j-1)):j*3,i)=J_s*V_S(1+(3*(j-1)):j*3,i);
    P(1+(3*(j-1)):j*3,i+1)=P(1+(3*(j-1)):j*3,i)+dt*V(1+(3*(j-1)):j*3,i);
end
end
end
%% Grafy
%rychlosti
figure
plot(t,KSI_dot_S(1,:))
title("rychlost do x")
figure
plot(t,KSI_dot_S(2,:))
title("rychlost do y")
figure
plot(t,KSI_dot_S(3,:))
title("úhlová rychlost")
% polohy
figure
plot(t,KSI_1(1,:))
title("poloho v x")
figure
plot(t,KSI_1(2,:))
title("poloha v y")
figure
plot(t,KSI_1(3,:))
title("natočení")
% Animace
figure
title('Trajektorie vozítek')
xlabel('poloha v X_1 [m]')
ylabel('poloha v Y_1 [m]')
set(gca,'FontName','Technika Book')
hold on;
Voz=gobjects(size(G,1),1);
Voziky_S=zeros(size(G,1)*3,4);
for i=1:size(G,1)
    Voziky_S(1+(3*(i-1)):i*3,:)=MSC_S(1+(3*(i-1)):i*3,:)*[Voziky;zeros(1,4)]; 
    
end
for i = 1:length(t)
    time=i*dt;
    time=string(time);
%     watch=text(-6,-6,time);
    phi=KSI_1(3,i);
    J_sn=[cos(phi)    -sin(phi);
        sin(phi)    cos(phi)];
    Rohy_1=J_sn*Rohy_S;
    A=fill(Rohy_1(1,:)+KSI_1(1,i),Rohy_1(2,:)+KSI_1(2,i),'g') ;
    for j=1:size(G,1)
        Voziky_1(1+3*(j-1):2+3*(j-1),:)=J_sn*Voziky_S(1+3*(j-1):2+3*(j-1),:);
        if j == 1
            Voz(j)=fill(Voziky_1(1+3*(j-1),:)+P(1+3*(j-1),i),Voziky_1(2+3*(j-1),:)+P(2+3*(j-1),i),'r');
        else
    Voz(j)=fill(Voziky_1(1+3*(j-1),:)+P(1+3*(j-1),i),Voziky_1(2+3*(j-1),:)+P(2+3*(j-1),i),'b');
        end   
    end
    hold on, grid on
    plot(KSI_1(1,1:i),KSI_1(2,1:i),'b-')
    axis equal
    axis( [-1.5 1.5 -1.5 1.5]),
    pause(1/(10000*n))
    if t(i)==0||t(i)==35||t(i)==70||t(i)==90||t(i)==125
    
else
    delete(A)
    delete(Voz)
%     delete(watch)

    end  
end


