clear; close all

%% parameters and system definition

G = tf([1,10],[1,4,20]);

%% root locus

figure
rlocus(G)
K_P = 4.13;

%% close the loop

G_cl_p = feedback(K_P*G,1);

%% step

t_a = linspace(0,2,150);
y_step_p = step(G_cl_p,t_a);

%% integral compensation

C_i = tf(1.5*[1,5],[1,0]);

figure
rlocus(K_P*C_i*G)

%% PID controller design

C_pid = pidtune(G,'PIDF');

G_cl_pid = feedback(C_pid*G,1);

%% step simulation

G_cl_pi = feedback(K_P*C_i*G,1);

y_step_pi = step(G_cl_pi,t_a);
y_step_pid = step(G_cl_pid,t_a);

% plot
figure
plot(t_a,y_step_p,'linewidth',1.5);hold on
plot(t_a,y_step_pi,'linewidth',1.5)
plot(t_a,y_step_pid,'linewidth',1.5)
xlabel('time (s)')
ylabel('step response y(t)')
grid on
legend('P control','PI control','PIDF control')

% evaluate settling time via stepinfo
si_p = stepinfo(y_step_p,t_a);
fprintf('P settling time: %.3g\n',si_p.SettlingTime)
si_pi = stepinfo(y_step_pi,t_a);
fprintf('PI settling time: %.3g\n',si_pi.SettlingTime)
si_pid = stepinfo(y_step_pid,t_a);
fprintf('PID settling time: %.3g\n',si_pid.SettlingTime)

%% sinusoidal ramp reference

f_sr = @(t) sin(2*pi*t)+t;

y_sr = lsim(G_cl_pi,f_sr(t_a),t_a);

% plot
figure
plot(t_a,f_sr(t_a)); hold on
plot(t_a,y_sr);
xlabel('time (s)')
ylabel('sinusoidal ramp response')
grid on
legend('command','response')