clc,close all,clear,beep off;
%% Przyklad
s = tf('s');
G = (10*(s+5))/((s+15)*(s^2+8*s+20));
%--------------------------------------------------------------------------
ess_step = 10;
ess_ramp = 0;
zeta = 0.707;
%--------------------------------------------------------------------------
PM = rad2deg(atan((2*zeta)/(sqrt(-2*zeta^2+sqrt(1+4*zeta^4)))));
omega_gc = 2;
%--------------------------------------------------------------------------
if ess_ramp == 0
    ess_step = ess_step/100;  
    Kp = 1/ess_step;          
    Kc = Kp/dcgain(minreal(G));
else
    ess_ramp = ess_ramp/100;  
    Kv = 1/ess_ramp;          
    Kc = Kv/dcgain(minreal(s*G));
end
%--------------------------------------------------------------------------
[Mg,Theta,W] = bode(G,omega_gc);
%--------------------------------------------------------------------------
PM_r = PM*pi/180;       
Theta_r = Theta*pi/180; 
%--------------------------------------------------------------------------
tau_z=(1+Kc*Mg*cos(PM_r-Theta_r))/(-omega_gc*Kc*Mg*sin(PM_r-Theta_r));
tau_p=(cos(PM_r-Theta_r)+Kc*Mg)/(omega_gc*sin(PM_r-Theta_r));

C=Kc*(1+s*tau_z)/(1+s*tau_p);

%--------------------------------------------------------------------------
Go = minreal(G/(1+G));
Gz = minreal(G*C/(1+G*C));
%--------------------------------------------------------------------------
figure;
with_standard_size
subplot(1,2,1)
    bodemag(G,G*C);  
    grid on;      
    legend('bez kompensatora', 'z kompensatorem') 
subplot(1,2,2)
    step(Go,Gz); 
    grid on;                   
    legend('bez kompensatora', 'z kompensatorem') 
