% mtc_630_prstencový motor
% dynamika prenosove funkce
% Posledni uprava: 2.3.2015

close all;
clear all;
clc;

% ----- Vytvoreni matic tuhosti a hmotnosti -----

m=[0.05 1.2 2.99];       % diskretni hmotnosti [kg.m2]
%k=[0 10e7 10e8]; % diskretni tuhosti [N/m]
p=1.8;
k2=1e7;  %tuhost høídele
k1=3e5; %tuhost remene
x=linspace(1, length(m), length(m)); % polohy pro zobrazeni diskretnich hmot [mm]

M=diag(m);
K=[k2          -k2                0 ;
  -k2      k2+k1/(p^2)         -k1/p;
   0          -k1/p              k1];
          
 
% ----- Vypocet netlumene modalky matic z matic K a M -----
[Un,Ln]=eig(K,M); % vypocet vl. cisel a vektoru (vl. vektory jsou ve sloupkach)
[L,j]=sort(diag(Ln)); % setrideni vl. cisel a ulozeni informace o trideni
LAM=round(abs(diag(L))); % spektralni matice
% U=zeros(size(Un)); % zalozeni prazdne matice na setridene nenormovane vl. vektory
% V=zeros(size(Un));
U=Un(:,j);
Mn=U'*M*U;
V=U*(Mn)^(-.5);
% for k=1:length(j)
%     U(:,k)=Un(:,j(k)); % setrideni vl. vektoru podle vl. cisel
%     V(:,k)=U(:,k)./sqrt(U(:,k)'*M*U(:,k)); % normovani vl. vektoru a jejich zapis do matice
% end

% ----- Kontrola ortogonality -----
disp(['Ortonormalita norm VT*M*V-E: ',num2str(norm(V'*M*V-eye(length(K))))]);
disp(['Ortonormalita norm VT*K*V-L: ',num2str(norm(V'*K*V-LAM))]);

% ----- Vypis vl. frekvenci -----
freq=round(1/(2*pi)*sqrt(L)) % prevod vl. cisel na frekvenci v [Hz]

% ----- Vykresleni vl. tvaru -----
figure(1)
title('Vlastni tvary');
for i=1:3
    subplot(3,2,i);
    plot(x,V(:,i),'k-','LineWidth',2);
    grid on; zoom on; hold on;
    xlabel('Poloha');
    ylabel('Vychylka');
    legend([num2str(i),'. tvar: ',num2str(freq(i)),'Hz'],0);
end

% ----- Sestaveni  matice tlumeni Bt -----
dz=[0.01 0.01 0.01]; % vektor pomernych utlumu
ii=2; jj=3; % pro vypocet alfa a beta se pouzije ii-ta a jj-ta frekvence

beta=(2*dz(ii)*freq(ii)-2*dz(jj)*freq(jj))/(freq(ii)^2-freq(jj)^2);
alfa=2*dz(ii)*freq(ii)-beta*(freq(ii)^2);

Bt=alfa*M+beta*K; % matice absolutnich utlumu

% ----- Prevod do stavoveho prostoru -----

OMEGA=sqrt(diag(LAM));    % vektor vl. frekvenci
DELTA=diag(2*dz.*OMEGA');  % matice relativnich utlumu
As=[zeros(length(K)) eye(length(K)); -1*LAM -1*DELTA];
Bs=[zeros(length(K)); V'];
Cs=V*[eye(length(K)) zeros(length(K))];
Ds=zeros(length(K));

SYS=ss(As,Bs,Cs,Ds); % definice systemu pomoci stavovych matic
Gs = tf(SYS);    % vypocet matice prenosu

% vektor budicich frekvenci
ff=(1:1:4000);

% vypocet amplitudy a faze pro definovany frekv. rozsah
% v definici prenosu je na prvnim miste cislo souradnice odezvy
% na druhem miste cislo souradnice sily

[magobr,phobr]=bode(Gs(2,2),2*pi*ff);
magobr=squeeze(magobr)';
phobr=squeeze(phobr)';

figure(2)
bode(Gs(2,2),2*pi*ff); grid on, hold on

% vypocet realne a imaginarni slozky prenosu
phobrrad=phobr.*(pi/180);
Gobr=magobr.*cos(phobrrad);
Hobr=magobr.*sin(phobrrad);

% vykresleni prenosu
lw=2;

figure(1)
  subplot(3,1,3);
plot(ff,magobr,'r-','LineWidth',lw);
grid on; zoom on; hold on;
title('Frekvencni prenosova funkce');
xlabel('Frekvence [Hz]');
ylabel('Log. amplituda [m]');
% legend('Fyzicke souradnice','Stavovy prostor',1);