%% Simulation / Asservissement d'un système
%   Module : Ingénierie Electronique 
%       pour le Traitement de l'Information
%--------------------------------------------------------------
%   TD_11 : Asservissement / Deuxième ordre
%--------------------------------------------------------------
%   Auteur : Julien VILLEMEJANE
%   Date : 12/12/2020
%--------------------------------------------------------------
clear all;
close all;
f = logspace(0,6,1001);
w = 2*pi.*f;
t = linspace(0,2e-2,1001);

%% Boucle de retour
K = 1;
B0 = K;     % gain

%% Système du second ordre
G0 = 1;
m = [0.2 0.4 0.7 1.5 2];
w0 = 2e3;

%% Correcteur 
G = 2;
tau_PI = 1e-4;

%% Système en boucle ouverte
num_m = max(size(m));
TF_2 = tf(zeros(1,1,num_m));
num_2 = [G0];
for i = 1:num_m
    den_2 = [1/(w0*w0) 2*m(i)/w0 1];
    TF_2(:,:,i) = tf(num_2, den_2);
end
% Affichage des N réponses indicielles
str_titre = 'Systeme_Ordre 2__Boucle_Ouverte';
figure('Name', str_titre, 'NumberTitle','off');
subplot(3,1,1);
for i = 1:num_m
    [y, t] = step(TF_2(:,:,i), t);
    str(i) = 'm = '+string(m(i));
    plot(t, y);
    legend(str);
    hold on;
end
xlabel('Temps(s)');
ylabel('Signal');
title('Reponse indicielle');
grid on;
subplot(3,1,2);
for i = 1:num_m
    [mag, phase, wout] = bode(TF_2(:,:,i), w);
    semilogx(w, 20*log10(squeeze(mag)));
    hold on;
end
xlabel('Fréquence (Hz)');
ylabel('Gain (dB)');
title('Diagramme de Bode - Gain');
grid on;
subplot(3,1,3);
for i = 1:num_m
    [mag, phase, wout] = bode(TF_2(:,:,i), w);
    semilogx(w, squeeze(phase));
    hold on;
end
xlabel('Fréquence (Hz)');
ylabel('Phase (deg)');
title('Diagramme de Bode - Phase');
grid on;

%% Système en boucle fermée sans correction avec K = 1
TF_BF_2 = tf(zeros(1,1,num_m));
for i = 1:num_m
    den_2 = [1/(w0*w0) 2*m(i)/w0 1];
    TF_BF_2(:,:,i) = K*feedback(TF_2(:,:,i), B0);
end
% Affichage des N réponses indicielles
str_titre = 'Systeme_Ordre 2__Boucle_Fermee';
figure('Name', str_titre, 'NumberTitle','off');
subplot(3,1,1);
for i = 1:num_m
    [y, t] = step(TF_BF_2(:,:,i), t);
    str_BF(i) = 'm = '+string(m(i));
    plot(t, y);
    legend(str_BF);
    hold on;
end
xlabel('Temps(s)');
ylabel('Signal');
title('Reponse indicielle');
grid on;
subplot(3,1,2);
for i = 1:num_m
    [mag, phase, wout] = bode(TF_BF_2(:,:,i), w);
    semilogx(w, 20*log10(squeeze(mag)));
    hold on;
end
xlabel('Fréquence (Hz)');
ylabel('Gain (dB)');
title('Diagramme de Bode - Gain');
grid on;
subplot(3,1,3);
for i = 1:num_m
    [mag, phase, wout] = bode(TF_BF_2(:,:,i), w);
    semilogx(w, squeeze(phase));
    hold on;
end
xlabel('Fréquence (Hz)');
ylabel('Phase (deg)');
title('Diagramme de Bode - Phase');
grid on;
%% Système corrigé avec Gain
CorP = tf([G],[1]);
TF_BF_2_C0 = tf(zeros(1,1,num_m));
for i = 1:num_m
    TF_BF_2_C0(:,:,i) = K*feedback(CorP*TF_2(:,:,i), B0);
end
% Affichage des N réponses indicielles
str_titre = 'Systeme_Ordre 2__Boucle_Fermee_Corrigee_P';
figure('Name', str_titre, 'NumberTitle','off');
subplot(3,1,1);
for i = 1:num_m
    [y, t] = step(TF_BF_2_C0(:,:,i), t);
    str_BFC0(i) = 'm = '+string(m(i));
    plot(t, y);
    legend(str_BFC0);
    hold on;
end
xlabel('Temps(s)');
ylabel('Signal');
title('Reponse indicielle');
grid on;
subplot(3,1,2);
for i = 1:num_m
    [mag, phase, wout] = bode(TF_BF_2_C0(:,:,i), w);
    semilogx(w, 20*log10(squeeze(mag)));
    hold on;
end
xlabel('Fréquence (Hz)');
ylabel('Gain (dB)');
title('Diagramme de Bode - Gain');
grid on;
subplot(3,1,3);
for i = 1:num_m
    [mag, phase, wout] = bode(TF_BF_2_C0(:,:,i), w);
    semilogx(w, squeeze(phase));
    hold on;
end
xlabel('Fréquence (Hz)');
ylabel('Phase (deg)');
title('Diagramme de Bode - Phase');
grid on;

%% Système corrigé avec PI
CorI = tf([1],[tau_PI 0]);
TF_BF_2_C1 = tf(zeros(1,1,num_m));
for i = 1:num_m
    TF_BF_2_C1(:,:,i) = K*feedback((CorP+CorI)*TF_2(:,:,i), B0);
end
% Affichage des N réponses indicielles
str_titre = 'Systeme_Ordre 2__Boucle_Fermee_Corrigee_PI';
figure('Name', str_titre, 'NumberTitle','off');
subplot(3,1,1);
for i = 1:num_m
    [y, t] = step(TF_BF_2_C1(:,:,i), t);
    str_BFC1(i) = 'm = '+string(m(i));
    plot(t, y);
    legend(str_BFC1);
    hold on;
end
xlabel('Temps(s)');
ylabel('Signal');
title('Reponse indicielle');
grid on;
subplot(3,1,2);
for i = 1:num_m
    [mag, phase, wout] = bode(TF_BF_2_C1(:,:,i), w);
    semilogx(w, 20*log10(squeeze(mag)));
    hold on;
end
xlabel('Fréquence (Hz)');
ylabel('Gain (dB)');
title('Diagramme de Bode - Gain');
grid on;
subplot(3,1,3);
for i = 1:num_m
    [mag, phase, wout] = bode(TF_BF_2_C1(:,:,i), w);
    semilogx(w, squeeze(phase));
    hold on;
end
xlabel('Fréquence (Hz)');
ylabel('Phase (deg)');
title('Diagramme de Bode - Phase');
grid on;

%% Pour un m
for i = 1:num_m
    str_m(i) = 'Systeme_Ordre_2__m_'+string(m(i));
    figure('Name',str_m(i));
    [yF, tF] = step(TF_BF_2(:,:,i), t);
    [y0, tF] = step(TF_BF_2_C0(:,:,i), t);
    [y1, tF] = step(TF_BF_2_C1(:,:,i), t);
    subplot(3,1,1);
    plot(t, yF, t, y0, t, y1);
    xlabel('Temps (s)'); ylabel('Signal'); grid on;
    [magF, phaseF, wout] = bode(TF_BF_2(:,:,i), w);
    [mag0, phase0, wout] = bode(TF_BF_2_C0(:,:,i), w);
    [mag1, phase1, wout] = bode(TF_BF_2_C1(:,:,i), w);
    subplot(3,1,2);
    semilogx(w, 20*log10(squeeze(magF)), w, 20*log10(squeeze(mag0)), w, 20*log10(squeeze(mag1)));
    xlabel('Fréquence (Hz)'); ylabel('Gain (dB)'); grid on;
    legend('Fermée','Cor P','Cor PI');
    subplot(3,1,3);
    semilogx(w, squeeze(phaseF), w, squeeze(phase0), w, squeeze(phase1));
    ylabel('Phase (deg)'); grid on;
end