clc, clear, close all

% C(s) = k*(s+z1)/(s+p1)    : compensator
% G(s) = b0/(s^2+a1*s+a0)   : plant
% C(s)G(s) = (k*b0*s + k*b0*z1)/(s^3 + (a1+p1)*s^2 + (a0+p1*a1)*s + p1*a0) 
%
% closed-loop TF :
% G_cl(s) = (k*b0*s + k*b0*z1)/(s^3 + (a1+p1)*s^2 + (a0+p1*a1+k*b0)*s + p1*a0+k*b0*z1)

k = 0.1;
z1 = 5.0;
p1 = 0.005;
b0 = 38.0818;
a1 = 2.0884;
a0 = 36.6846;

open_sys = tf(b0, [1 a1 a0]);
display(open_sys);

CG_num = [k*b0 k*b0*z1];
CG_den = [1 a1+p1 a0+p1*a1 p1*a0];
CG_sys = tf(CG_num, CG_den);
display(CG_sys);
rlocus(CG_sys);

closed_num = CG_num;
closed_den = [1 a1+p1 a0+p1*a1+k*b0 p1*a0+k*b0*z1];
closed_sys = tf(closed_num, closed_den);
display(closed_sys);
c_poles = pole(closed_sys);
display(c_poles);