%% CDSSC - Examen Practico 1 (18/11/2025) - full solution script
% Run section by section (Ctrl+Enter). Every number you write on paper comes from here.
clear; close all; clc
s = tf('s');
G = 500/(s*(s+500));          % motor: voltage -> position
Ks = [50 125 1000];

%% Apartado 1.1 - Root locus with the three K marked
figure; rlocus(G); hold on
for K = Ks
    p = pole(feedback(K*G,1));                  % closed-loop poles for this K
    plot(real(p), imag(p), 'o', 'MarkerSize',8, 'LineWidth',2)
    text(real(p(1)), imag(p(1))+40, sprintf('K=%d',K))
    fprintf('K=%4d  poles: %s\n', K, mat2str(p.',4))
end
title('Root locus of G(s), P control'); grid on
% Breakaway point: rlocus click, or analytically s=-250 at K=125

%% Apartado 1.2 - Step responses + Mp, tp, ts(5%), y(inf)
figure; hold on
for K = Ks
    T = feedback(K*G,1);
    step(T)
    i = stepinfo(T,'SettlingTimeThreshold',0.05);
    [wn,z] = damp(T);
    fprintf('K=%4d  zeta=%.3f wn=%.1f  Mp=%.1f%%  tp=%.4f s  ts5=%.4f s  y_inf=%.3f\n', ...
        K, z(1), wn(1), i.Overshoot, i.PeakTime, i.SettlingTime, dcgain(T))
end
legend('K=50','K=125','K=1000'); grid on
% NOTE: when Overshoot = 0 there is no peak -> tp does not exist (stepinfo then
% reports the time it reaches the final value; write "no hay pico" on paper).

%% Apartado 1.3 - Relative stability: margins on L, peak of S (Ms), peak of T (Mt)
figure; hold on
for K = Ks
    L = K*G;
    S = feedback(1,L);        % S = 1/(1+L)
    T = feedback(L,1);        % T = L/(1+L)
    [Gm,Pm,Wcg,Wcp] = margin(L);
    [Ms,wMs] = getPeakGain(S);
    [Mt,wMt] = getPeakGain(T);
    fprintf('K=%4d  MF=%.1f deg @ %.1f rad/s  MG=%s  Ms=%.3f (%.2f dB) @ %.1f  a=1/Ms=%.3f  Mt=%.3f (%.2f dB)\n', ...
        K, Pm, Wcp, num2str(20*log10(Gm)), Ms, 20*log10(Ms), wMs, 1/Ms, Mt, 20*log10(Mt))
    bodemag(S)
end
legend('S, K=50','S, K=125','S, K=1000'); grid on; title('|S(j\omega)|')
figure; for K = Ks, margin(K*G); hold on; end   % margins for each K
figure; for K = Ks, nyquist(K*G); hold on; end  % distance from -1 = 1/Ms
axis([-2 0.5 -2 2])

%% Apartado 2 - PI controllers
for K = Ks
    D = K*(s+100)/s;
    L = D*G;
    S  = feedback(1,L);           % e/r   (tracking error)
    Si = feedback(G,D);           % y/d   = G/(1+DG)  (input disturbance -> output)
    Su = feedback(D,G);           % u/r   = D/(1+DG)
    T  = feedback(L,1);
    fprintf('\nPI K=%d  stable: %d  poles: %s\n', K, isstable(T), mat2str(pole(T).',4))
    % 2.1 position error: e_p = S(0)*1 (step r=1)
    fprintf('  e_p = S(0) = %.4g\n', dcgain(S))
    % 2.2 constant disturbance d=5 -> y(inf) = Si(0)*5
    fprintf('  y_inf from d=5: %.4g\n', 5*dcgain(Si))
    % 2.3 sinusoidal disturbance A=2, w=50 -> ripple amplitude = A*|Si(j50)|
    fprintf('  |Si(j50)| = %.4g  -> ripple amplitude = %.4g\n', abs(evalfr(Si,50j)), 2*abs(evalfr(Si,50j)))
    [Mi,wMi] = getPeakGain(Si);
    fprintf('  peak |Si| = %.4g at w = %.1f rad/s -> worst ripple = %.4g\n', Mi, wMi, 2*Mi)
end
figure; for K = Ks, bodemag(feedback(G,K*(s+100)/s)); hold on; end
legend('Si, K=50','Si, K=125','Si, K=1000'); grid on; title('|S_i(j\omega)| with PI')
% Verify the ripple in time: lsim with d = 2 sin(50t), r = 0
t = 0:1e-4:1; d = 2*sin(50*t);
figure; for K = Ks, lsim(feedback(G,K*(s+100)/s), d, t); hold on; end
legend('K=50','K=125','K=1000'); title('y(t) caused by d = 2 sin(50t)')