% Kinematicka analyza klukoveho mechanizmu pomocou maticovej metody
clear all
clc

% Rozmery
a = 0.3;
b = 0.7;
l = a + b;
r = 1.5;

% Polohy bodov s SS jednotlivych telies
r_1A = [0 0 0 1]';
r_2B = [a 0 0 1]';
r_3L = [r 0 0 1]';
r_4C = [-0.15 0 0 1]';
r_4D = [0.15 0 0 1]';

% Casovy rozsah
dt = 0.05;           %[s] casova oblast
t_end = 1;           %[s] casovy krok
time = 0:dt:t_end;   %[s] casovy vektor
t_steps = length(time);

% Nezavisle suradnice
phi = 2*pi*time;
dphi = 2*pi;
ddphi = 0;

% Startovacie hodnoty pre vypocet polohy
theta0 = 0;
psi0 = 0;
x0 = 0;
pos0 = [theta0 psi0 x0];

i = 1;
for t = time
    % Polohy zavislych suradnic
    par = [a b l phi(i)];
    % Riesenie systemu nelinearnych algebraickych rovnic
    pos = fsolve(@FCN_kluk_mech,pos0,[],par);
    theta(i) = pos(1);
    psi(i) = pos(2);
    x(i) = pos(3);
    % Transformacne matice a maticove diferencialne operatory zakl. pohybov
    [T1, D1] = FCN_basic_mot(phi(i), 6);
    [T2, D2] = FCN_basic_mot(a, 1);
    [T3, D3] = FCN_basic_mot(theta(i), 6);
    [T4, D4] = FCN_basic_mot(l, 1);
    [T5, D5] = FCN_basic_mot(psi(i), 6);
    [T6, D6] = FCN_basic_mot(-b, 1);
    [T7, D7] = FCN_basic_mot(x(i), 1);    
    
    T12 = T1;
    T23 = T2*T3;
    T14 = T4*T5;
    
    % Poloha bodov mechanizmu
    r_1B(:,i) = T12*r_2B;
    r_1L(:,i) = T12*T23*r_3L;
    r_1C(:,i) = T14*r_4C;
    r_1D(:,i) = T14*r_4D;
    
    % Rychlosti zavislych suradnic
    M = [-cos(phi(i))*cos(theta(i))+sin(phi(i))*sin(theta(i)) cos(psi(i))           0;
          0                                                  -(b-x(i))*sin(psi(i)) -cos(psi(i));
          0                                                   (b-x(i))*cos(psi(i)) -sin(psi(i))];
    N = [cos(phi(i))*cos(theta(i))-sin(phi(i))*sin(theta(i));
         a*sin(phi(i));
        -a*cos(phi(i))];
    % Riesenie systemu linearnych algebraickych rovnic
    vel = M\N*dphi;
    dtheta(i) = vel(1);
    dpsi(i) = vel(2);
    dx(i) = vel(3);
    % Rychlost bodu L
    W1 = T1*D1*T2*T3;
    W2 = T1*T2*D2*T3;
    W3 = T1*T2*T3*D3;
    v_1L(:,i) = (W1*dphi + W2*0 + W3*dtheta(i))*r_3L;
    
    % Zrychlenia zavislych suradnic
    L = [sin(psi(i))*dpsi(i)^2 - (cos(phi(i))*sin(theta(i))+cos(theta(i))*sin(phi(i)))*(dphi^2+dtheta(i)^2+2*dphi*dtheta(i));
         a*cos(phi(i))*dphi^2 + (b-x(i))*cos(psi(i))*dpsi(i)^2 - 2*sin(psi(i))*dx(i)*dpsi(i);
         a*sin(phi(i))*dphi^2 + (b-x(i))*sin(psi(i))*dpsi(i)^2 + 2*cos(psi(i))*dx(i)*dpsi(i)];
    % Riesenie systemu linearnych algebraickych rovnic 
    acc = M\(L+N*ddphi);
    ddtheta(i) = acc(1);
    ddpsi(i) = acc(2);
    ddx(i) = acc(3); 
    % Zrychlenie bodu L
    W11 = T1*D1*D1*T2*T3;
    W12 = T1*D1*T2*D2*T3;
    W13 = T1*D1*T2*T3*D3;
    W21 = T1*D1*T2*D2*T3;
    W22 = T1*T2*D2*D2*T3;
    W23 = T1*T2*D2*T3*D3;
    W31 = T1*D1*T2*T3*D3;
    W32 = T1*T2*D2*T3*D3;
    W33 = T1*T2*T3*D3*D3;
    a_1L(:,i) = (W1*ddphi + W11*dphi*dphi + W12*dphi*0 + W13*dphi*dtheta(i) + ...
                 W2*0 + ...
                 W3*ddtheta(i) + W31*dtheta(i)*dphi + W32*dtheta(i)*0 + W33*dtheta(i)*dtheta(i))*r_3L;
    
    % Startovacie hodnoty pre novy krok
    pos0 = [theta(i) psi(i) x(i)];   
    i = i + 1;  
end

figure(1), clf
    subplot(3,1,1)
plot(time,r_1L(1,:),'Linewidth',2), hold on
title('Poloha bodu L')
xlabel('t [s]'), ylabel('x_{1L} [m]')
grid on
    subplot(3,1,2)
plot(time,v_1L(1,:),'Linewidth',2), hold on
title('Rychlost bodu L')
xlabel('t [s]'), ylabel('vx_{1L} [m/s]')
grid on
    subplot(3,1,3)
plot(time,a_1L(1,:),'Linewidth',2), hold on
title('Zrychlenie bodu L')
xlabel('t [s]'), ylabel('ax_{1L} [m/s^2]')
grid on    

% Polohy bodov v SS ramu
Ax = r_1A(1);   Ay = r_1A(2);
Bx = r_1B(1,:); By = r_1B(2,:);
Lx = r_1L(1,:); Ly = r_1L(2,:);
Cx = r_1C(1,:); Cy = r_1C(2,:);
Dx = r_1D(1,:); Dy = r_1D(2,:);

% Animacia pracovneho cyklu mechanizmu
i = 1;
for t = time
    figure(2),clf
    plot([Ax Bx(i)],[Ay By(i)],'-o','LineWidth',8,'Markersize',9); hold on
    plot([Bx(i) Lx(i)],[By(i) Ly(i)],'-or','LineWidth',8,'Markersize',9); hold on
    plot([Cx(i) Dx(i)],[Cy(i) Dy(i)],'k','LineWidth',20);
    plot(l,0,'ow','LineWidth',8,'Markersize',5)
    plot(r_1L(1,:),r_1L(2,:),'r')
    title('Animacia pohybu mechanizmu')
    xlabel('x [m]'), ylabel('y [m]')
    axis([-0.4 2 -1.2 1.2])
    axis square
    i = i + 1;
end