% dvehmoty.m ... program k prikladu na kmitani se dvema stupni volnosti
%                na webu http://boletus.fsik.cvut.cz/benes

% Do referatu ME III a VME je potreba pouze vypocet vlastnich frekvenci a
% vlastnich tvaru, vykresleni r1 a r2 v zavislosti na menici se budici
% frekvenci a graf celkoveho reseni pro dane pocatecni podminky

clear all
close all

% Zadane hodnoty
I1=3.1
I2=1.6

k1=2000
k2=0
F10=14
om=6
                  
% pocatecni podminky
fi0=[-0.002;
     -0.004]
fi0dot=[0;
        0]
% Matice hmotnosti
M=[I1  0;
    0 I2];
% Matice tuhosti
K=[ 949.66+k1   -k1;
   -k1 k1];
% Amplituda budici sily
f0=[F10;
     0];

% HOMOGENNI RESENI
% vypocet vlastnich cisel a vektoru
[u,v]=eig(K,M)  % u...vl. vektory, v...vl.cisla
% vypocet vlastnich frekvenci
OM1=sqrt(v(1,1))
OM2=sqrt(v(2,2))
% normovani vlastnich vektoru na tvar popsany v prikladu na webu 
u1norm=u(:,1)/u(1,1)
u2norm=u(:,2)/u(1,2)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TAHLE CAST V REFERATECH NENI POTREBA ! %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Vypocet konstant volenho tlumeni pro zadane obecne pocatecni podminky
A=inv(u)*fi0
B=inv(u*sqrt(v))*fi0dot

% vykresleni prubehu volnych kmitu
t=0:0.01:2; % definujeme si nejaky casovy usek
% ... a napocteme prubeh vychylek
fi1=A(1)*u(1,1)*cos(OM1*t)+B(1)*u(1,1)*sin(OM1*t)+A(2)*u(1,2)*cos(OM2*t)+B(2)*u(1,2)*sin(OM2*t);
fi2=A(1)*u(2,1)*cos(OM1*t)+B(1)*u(2,1)*sin(OM1*t)+A(2)*u(2,2)*cos(OM2*t)+B(2)*u(2,2)*sin(OM2*t);

figure      % zalozi novy obrazek
plot(t,fi1,'b',t,fi2,'r')   % vykresleni prubehu
title('Homogenni reseni pro zadane obecne pocatecni podminky')
xlabel('cas [s]'), ylabel('vychylka [rad]')
legend('fi1(t)','fi2(t)')

% ukazka vybuzeni vlastnich tvaru
% pocatecni vychylky v pomeru 1. vl.tvaru
A1=inv(u)*[0.05*u(1,1); 0.05*u(2,1)]
B1=inv(u*sqrt(v))*[0; 0]
% ... a napocteme prubeh vychylek
fi1=A1(1)*u(1,1)*cos(OM1*t)+B1(1)*u(1,1)*sin(OM1*t)+A1(2)*u(1,2)*cos(OM2*t)+B1(2)*u(1,2)*sin(OM2*t);
fi2=A1(1)*u(2,1)*cos(OM1*t)+B1(1)*u(2,1)*sin(OM1*t)+A1(2)*u(2,2)*cos(OM2*t)+B1(2)*u(2,2)*sin(OM2*t);
figure      % zalozi novy obrazek
plot(t,fi1,'b',t,fi2,'r')   % vykresleni prubehu
title('Vybuzeni 1. vlastniho tvaru pocatecnimi vychylkami')
xlabel('cas [s]'), ylabel('vychylka [rad]')
legend('fi1(t)','fi2(t)')

t_1_tvar=t;
fi1_1_tvar=fi1;
fi2_1_tvar=fi2;

% pocatecni rychlosti v pomeru 2. vl.tvaru
A2=inv(u)*[0; 0]
B2=inv(u*sqrt(v))*[2*u(1,2); 2*u(2,2)]
% ... a napocteme prubeh vychylek
fi1=A2(1)*u(1,1)*cos(OM1*t)+B2(1)*u(1,1)*sin(OM1*t)+A2(2)*u(1,2)*cos(OM2*t)+B2(2)*u(1,2)*sin(OM2*t);
fi2=A2(1)*u(2,1)*cos(OM1*t)+B2(1)*u(2,1)*sin(OM1*t)+A2(2)*u(2,2)*cos(OM2*t)+B2(2)*u(2,2)*sin(OM2*t);
figure      % zalozi novy obrazek
plot(t,fi1,'b',t,fi2,'r')   % vykresleni prubehu
title('Vybuzeni 2. vlastniho tvaru pocatecnimi rychlostmi')
xlabel('cas [s]'), ylabel('vychylka [rad]')
legend('fi1(t)','fi2(t)')

t_2_tvar=t;
fi1_2_tvar=fi1;
fi2_2_tvar=fi2;



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TOHLE UZ ZASE V REFERATECH JE ... %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% PARTIKULARNI RESENI - vynucene kmity
% amplituda vynucenych kmitu pro zadanou frekvenci buzeni omega
R=inv(K-om^2*M)*f0

% vypocet amplitud v zavislosti na menicim se omega
r=[];   % pripravim si prazdne pole, do ktereho budu ukladat r jako funkci omega
omega=0:0.1:60;
for i=1:1:length(omega)
    r=[r, inv(K-omega(i)^2*M)*f0];
end
% vykresleni
figure
plot(omega, r(1,:),'b.',omega, r(2,:),'r.')
axis([0 60 -1 1])   % upravuji si osy na obrazku, protoze pri rezonanci utikaji amplitudy k nekonecnu 
grid on
title('Amplitudove charakteristiky')
xlabel('omega [1/s]'), ylabel('vychylka [rad]')
legend('r1','r2')

% CELKOVE RESENI
% konstanty pro celkove reseni urcime z pocatecnich podminek
A=inv(u)*fi0
B=inv(u*sqrt(v))*(fi0dot-R*om)
% vykresleni prubehu kmitu
t=0:0.0001:6*pi/OM1; % definujeme si nejaky casovy usek
% ... a napocteme prubeh vychylek
fi1=A(1)*u(1,1)*cos(OM1*t)+B(1)*u(1,1)*sin(OM1*t)+A(2)*u(1,2)*cos(OM2*t)+B(2)*u(1,2)*sin(OM2*t)+R(1)*sin(om*t);
fi2=A(1)*u(2,1)*cos(OM1*t)+B(1)*u(2,1)*sin(OM1*t)+A(2)*u(2,2)*cos(OM2*t)+B(2)*u(2,2)*sin(OM2*t)+R(2)*sin(om*t);
% vykresleni
figure      % zalozi novy obrazek
plot(t,fi1,'b',t,fi2,'r')   % vykresleni prubehu
title('Celkove reseni pro zadane obecne pocatecni podminky')
xlabel('cas [s]'), ylabel('vychylka [rad]')
legend('fi1(t)','fi2(t)')


% animace
figure
for i=1:length(t_1_tvar)
    plot(0,fi1_1_tvar(i),'rs','linewidth',8); hold on
    plot(0,fi2_1_tvar(i)-0.02,'rs','linewidth',8); 
    plot([0,0],[0.04,fi1_1_tvar(i)],'r','linewidth',(0.04+fi1_1_tvar(i))*100);
    plot([0,0],[fi1_1_tvar(i),fi2_1_tvar(i)-0.02],'r','linewidth',(fi1_1_tvar(i)+fi2_1_tvar(i)+0.05)*100);
    axis([-1 1 -0.06 0.04]);
    pause(0.01)
    hold off
    drawnow
end

pause
figure
for i=1:length(t_2_tvar)
    plot(0,fi1_2_tvar(i),'bs','linewidth',8); hold on
    plot(0,fi2_2_tvar(i)-0.05,'bs','linewidth',8); 
    plot([0,0],[0.04,fi1_2_tvar(i)],'b','linewidth',(0.04+fi1_2_tvar(i))*100);
    plot([0,0],[fi1_2_tvar(i),fi2_2_tvar(i)-0.05],'b','linewidth',(fi1_2_tvar(i)+fi2_2_tvar(i)+0.05)*100);
    axis([-1 1 -0.09 0.04]);
    pause(0.01)
    hold off
    drawnow
end
    