%% PID control, UNSTABLE G(s)=1/s^2
% © 2024, Antonio Sala Piqueras, Universitat Politècnica de Valencia, Spain. All rights reserved.
% Objective: understand the behaviour of proportional-integral-derivative control (simulations in companion videos) on a double-integrator system.
% Videos:
% ENGLISH
%  https://personales.upv.es/asala/YT/V/dintteostEN.html
%  https://personales.upv.es/asala/YT/V/dintteoerrEN.html
% SPANISH
%  https://personales.upv.es/asala/YT/V/dintteost.html
%  https://personales.upv.es/asala/YT/V/dintteoerr.html
%
%% Closed-loop transfer functions
% In a closed-loop:
%
% The equations are:

syms G  K K_p K_d r u y n du e

LoopEquations={u == K*e,  y == G*(u+du), e == r-(y+n)};
sol=solve(LoopEquations, [u y e])

% We will be interested in output (controlled variable) trajectories:

collect(sol.y,[r du n])

% Or, well, error will be r-y, but, anyway, we can display it:

collect(sol.e,[r du n])

% And, later on, in manipulated variable trajectories:

collect(sol.u,[r du n])

%% Setpoint change response

syms s %Laplace variable
G=2/s^2
%K=Kc*(1+Td*s)
%Kp=Kc, Kd=Kc*Td
K=K_p+K_d*s
G*K/(1+G*K)
CLrefSYM(s)=collect(simplify(G*K/(1+G*K)),s)
ErrFromref=collect((1-CLrefSYM),s)

%% Disturbance input response

CLduSYM(s)=collect(simplify(G/(1+G*K)),s)

%
% Dynamics: We have "total controllability", i.e., capability of assigning the closed-loop poles at any arbitrary position by tweaking K_c and K_i. This is a standard "exam question" in starting courses in Control Theory. Say, we wish a time constant of 0.5 (poles at -2), then

[N,D]=numden(ErrFromref);
collect(D,s) %denominator is the same for any chosen output variable
cD=coeffs(D,s)
DesiredDenominator=(s+5)^2
cDD=coeffs(DesiredDenominator,s)
solve(cD==cDD,[K_p, K_d])

%
% In reverse, for a given controller, the closed-loop poles are at:

solve(D==0,s) %closed-loop poles

% This starts getting too complicated... We will switch to Control systems Toolbox.

s=tf('s');
G=2/s^2;

%
% 1. Small T_d 2. Medium T_d, "cancellation" controller, 3. Large T_d.

K_cmax=140; 
T_d=0.125; %case 1
%T_d=0.5;%case 2
%T_d=10/11;%case 3
%T_d=0 %only PROPORTINAL
K1=1*(1+T_d*s);
K_c_percent=0.75; Kc=K_c_percent*K_cmax
r=rlocus(G*K1,Kc)'
settlingtimeapprox=-4./real(r)
figure(), subplot(2,1,1), rlocus(G*K1), hold on, plot(real(r),imag(r),'*k',LineWidth=3)
hold off, axis equal, xlim([-3/T_d 0.4]), grid on, subplot(2,1,2)
CLref=feedback(G*K1*Kc,1);
step(CLref,6), grid on,legend("C.L.",Location="southeast")
figure()
CLuref=feedback(K1*Kc,G)

% No realizable, nunca funcionará en la práctica (necesitamos filtro de ruido).

figure()
CLdu=feedback(G,K1*Kc);
step(CLdu,6), grid on, legend("C.L.",Location="southeast")

%
%%