clc
clear all
close all

Rw = 5;
Lw = 0.1;
ke = 0.1;
J = 0.1;
B = 0.5;
km = 0.1;

T = 0.01;

Kp1 = 5;
Ti1 = 0.25;
Td1 = 0.05;

LGo = km;
MGo = [J*Lw Lw*B+Rw Rw*B+km*ke];

                         % Li   Mi
[ Lr1, Mr1 ] = parallel ( [1], [Ti1 0], [1], [1] );     % polaczenie I1 i 1
                          %Lr1      Mr1
[ Lr1, Mr1 ] = parallel ( [Lr1], [Mr1], [Td1 0], [T 1] ); % polaczenie wynikowego z D1
[ Lr1, Mr1 ] = series ( [Kp1], [1], [Lr1], [Mr1] );    % polaczenie wynikowego z Kp1

[ Lz1, Mz1 ] = series ( [LGo], [MGo], [Lr1], [Mr1] );
[ Lz1, Mz1 ] = feedback ( [Lz1], [Mz1], [1], [1] );

w = 0 : 0.01 : 10000;

Y = freqs ( LGo, MGo, w ); %odpowiedz czestotliwosciowa w postaci zespolonej

Y2 = freqs ( Lz1, Mz1, w ); %to samo, dla dzeta2

tf(LGo,MGo)
tf(Lz1,Mz1)

mag = abs ( Y ); %amplituda
phase = angle ( Y ); %radiany

mag2 = abs ( Y2 );
phase2 = angle ( Y2 );



%char. amp-fazowa:
figure('Color',[1 1 1],'Name','','Units','Centimeters','Position',[6 4 15 10]);
plot ( real(Y), imag(Y), 'k', real(Y2), imag(Y2), '--k', 'LineWidth', 2 );
title('Charakterystyka amplitudowo-fazowa dla ? = 1 [rad/s] T [s]');
xlabel('Re(Y)');
ylabel('Im(Y)');
legend('hgjhg');
grid on;

%char. logarytmiczna amplitudy:
figure('Color',[1 1 1],'Name','','Units','Centimeters','Position',[6 4 15 10]);
magdB = 20 * log10 ( mag );
magdB2 = 20 * log10 ( mag2 );
subplot(2, 2, [1 2]);
semilogx ( w, magdB, 'k', w, magdB2, '--k', 'LineWidth', 2 );
title('Logarytmiczna charakterystyka amplitudowa dla ? = 1 [rad/s]');
%xlabel('omega [rad/s]');
ylabel('amplituda [dB]');
grid on;

%char. logarytmiczna fazy:
phased = phase * (180/pi); % stopnie
phased2 = phase2 * (180/pi);
subplot(2, 2, [3 4]);
semilogx ( w, phased, 'k', w, phased2, '--k', 'LineWidth', 2 );
title('Logarytmiczna charakterystyka fazowa dla ? = 1 [rad/s]');
xlabel('omega [rad/s]');
ylabel('faza [°]');
grid on;