%% [BE] Digital control of cantilever beam

% Justin Bos
% Wissal Guarni
% Nolan Reynier Nomer
% Aleksander Taban
clear;
clc;
close all;

SAVE = 0;

set(0, 'DefaultLineLineWidth', 1)

%% Question 3

E = [13/35 9/70 11/210 -13/420 ;
     9/70 13/35 13/420 -11/210 ;
     11/210 13/420 1/105 -1/140 ;
     -13/420 -11/210 -1/140 1/105] ;

D = [-6/5 6/5 -1/10 -1/10 ;
     6/5 -6/5 1/10 11/10 ;
     -11/10 1/10 -2/15 1/30 ;
     -1/10 1/10 1/30 -2/15] ;

phi = @(zeta) [2*zeta^3-3*zeta^2+1 ; 
            3*zeta^2-2*zeta^3 ; 
            zeta^3-2*zeta^2+zeta ; 
            zeta^3-zeta^2 ] ; 

L = 1 ; % Longueur de la poutre
phi_L = phi(L) ;

A = [zeros(4), inv(E)*D ;
    -inv(E)*(D'), zeros(4) ] ;
B = [zeros(4,1) ; -inv(E)*phi_L] ;
C = [zeros(1,4) , -phi_L'] ;

A, B, C

%% Question 4
Ts1 = 0.01 ;
Ts2 = 0.02 ;
Ts3 = 0.04 ;

sys = ss(A, B, C, 0) ;

eig_cont = eig(sys);
eig_tustin1 = eig(c2d(sys, Ts1, 'tustin')) ;
eig_tustin2 = eig(c2d(sys, Ts2, 'tustin')) ;
eig_tustin3 = eig(c2d(sys, Ts3, 'tustin')) ;
eig_zoh1 = eig(c2d(sys, Ts1, 'zoh')) ;
eig_zoh2 = eig(c2d(sys, Ts2, 'zoh')) ;
eig_zoh3 = eig(c2d(sys, Ts3, 'zoh')) ;
%%
Question4 = figure ;

subplot(1,3,1) ;
plot(eig_cont, 'x');
axis square;
grid on;
xline(0, 'r--');
xlabel('Réel');
ylabel('Imaginaire');
title({'Valeurs propres en temps continu'});

subplot(1,3,2);
hold on;
plot(eig_tustin1,'x', 'LineWidth',1);
plot(eig_tustin2,'x', 'LineWidth',1);
plot(eig_tustin3,'x', 'LineWidth',1);
axis square;
theta = linspace(0, 2*pi, 100);
plot(cos(theta), sin(theta), 'k--', 'HandleVisibility','off','LineWidth',0.25);
xlim([-1.1, 1.1]);
ylim([-1.1, 1.1]);
grid on;
legend("Ts1", "Ts2", "Ts3");
xlabel('Réel');
ylabel('Imaginaire');
title({'Valeurs propres en temps discret',' avec la méthode de Tustin'});
hold off;

subplot(1,3,3);
hold on;
plot(eig_zoh1,'x', 'LineWidth',1);
plot(eig_zoh2,'x', 'LineWidth',1);
plot(eig_zoh3,'x', 'LineWidth',1);
axis square;
legend("Ts1", "Ts2", "Ts3");
plot(cos(theta), sin(theta), 'k--', 'HandleVisibility','off','LineWidth',0.25);
xlim([-1.1, 1.1]);
ylim([-1.1, 1.1]);
grid on;
title({'Valeurs propres en temps discret',' avec un bloqueur d''ordre 0'});
xlabel('Réel');
ylabel('Imaginaire');
hold off;

if SAVE
    exportgraphics(Question4, './latex/Illustrations/Question4.pdf')
end
%% Question 5

t = 0:0.01:10;
cmd = double (t >= 1) ;
[y, t] = lsim(sys,cmd,t) ;

Question5 = figure ;
plot(t, cmd, t, y);
xlabel("Temps (s)");
ylabel("Amplitude");
legend("$u(t)$", "$y(t)$", 'Interpreter', 'latex');
title("Simulation en boucle ouverte") ;

if SAVE
    exportgraphics(Question5, './latex/Illustrations/Question5.pdf')
end

%% Question 7

Cw = @(zeta) [zeta^2*(2*zeta^3-5*zeta^2+10)/20 ...
            -zeta^4*(2*zeta-5)/20 ...
              zeta^3*(3*zeta^2-10*zeta+10)/60 ...
              zeta^4*(3*zeta-5)/60 ...
              0, 0, 0, 0];
Cw_L = Cw(L);
%%

TransientTime = zeros(size(0.01:0.01:10)) ;
for i = 1:size(TransientTime, 2)
    TransientTime(i) = stepinfo(feedback(sys, i*0.01)).TransientTime ;
end
[minimum, min_index] = min(TransientTime)
k = min_index * 0.01 

H = inv(Cw_L*inv(-A+B*C*k)*B)

%% Question8
[y, t, x] = lsim(feedback(sys,k), H*cmd, t) ;
u = H*cmd'-k*y ;

Question8 = figure ;
plot(t, u, t, y, t, cmd, t, (Cw_L*x')');
xlabel("Temps (s)");
ylabel("Amplitude");
legend("$u(t)$", "$y(t)$", "$w_c(L,t)$", "$w(L,t)$", 'Interpreter', 'latex');
title("Simulation du retour de sortie avec pré-gain") ;

if SAVE
    exportgraphics(Question8, './latex/Illustrations/Question8.pdf')
end

%% Question 9


eig_cont_CL = eig(feedback(sys,k));
eig_zoh1_CL = eig(feedback(c2d(sys, Ts1, 'zoh'), k)) ;
eig_zoh2_CL = eig(feedback(c2d(sys, Ts2, 'zoh'), k)) ;
eig_zoh3_CL = eig(feedback(c2d(sys, Ts3, 'zoh'), k)) ;

%%

Question9 = figure;
subplot(1,2,1) ;

plot(eig_cont_CL, 'x');
hold on;
axis square;
grid on;
xline(0, 'r--');
title({'Valeurs propres en temps continu'});
xlabel('Réel');
ylabel('Imaginaire');
hold off;

subplot(1,2,2);

plot(eig_zoh1_CL,'x');
hold on;
plot(eig_zoh2_CL,'x');
plot(eig_zoh3_CL,'x');
axis square;
plot(cos(theta), sin(theta), 'k--', 'HandleVisibility','off','LineWidth',0.25);
xlim([-1.5, 1.5]);
ylim([-1.5, 1.5]);

grid on;
legend("Ts1", "Ts2", "Ts3");
title({'Valeurs propres en temps discret',' avec un bloqueur d''ordre 0'});
xlabel('Réel');
ylabel('Imaginaire');
hold off;

if SAVE
    exportgraphics(Question9, './latex/Illustrations/Question9.pdf')
end

%%

for Ts = Ts2:0.001:Ts3
    if all(abs(eig(feedback(c2d(sys,Ts,'zoh'),k))) < 1)
        ;
    else
        break;
    end
    
end
% Maximum Sampling period
Ts = Ts-0.001
%%
sys_CL_Ts = feedback(c2d(sys, Ts, 'zoh'),k);
t_d = 0:Ts:10 ;
cmd_d = double (t_d >= 1) ;
[y, t_d, x_d] = lsim(sys_CL_Ts, H*cmd_d, t_d) ;
u_d = H*cmd_d'-k*y ;

Question10 = figure ;
plot(t_d, u_d, t_d, y, t_d, cmd_d, t_d, (Cw_L*x_d')');
xlabel("Temps (s)");
ylabel("Amplitude");
legend("$u(t)$", "$y(t)$", "$w_c(L,t)$", "$w(L,t)$", 'Interpreter', 'latex');
title({'Simulation du retour de sortie avec pré-gain','avec un bloqueur d''ordre 0'}) ;

if SAVE
    exportgraphics(Question10, './latex/Illustrations/Question10.pdf')
end

%% Question 11

K_lqr = lqr(sys, eye(size(A)), 1);
K_lqr
H_lqr = inv(Cw_L*inv(-A+B*K_lqr)*B) ;

%% Question 12

sys_CL_lqr = ss(A-B*K_lqr, B*H_lqr, C, 0);
[y, t, x] = lsim(sys_CL_lqr, cmd, t);
u = H_lqr*cmd'-(K_lqr*x')' ;
Question12 = figure ;
plot(t, u, t, y, t, cmd, t, (Cw_L*x')');
xlabel("Temps (s)");
ylabel("Amplitude");
legend("$u(t)$", "$y(t)$", "$w_c(L,t)$", "$w(L,t)$", 'Interpreter', 'latex');
title("Simulation du retour d'états avec pré-gain") ;

if SAVE
    exportgraphics(Question12, './latex/Illustrations/Question12.pdf')
end

%% Question 13

for Ts = 0.01:0.0001:0.02
    if all(abs(eig(c2d(sys,Ts,'zoh').A-c2d(sys,Ts,'zoh').B*K_lqr)) < 1)
        ;
    else
        break;
    end
end
Ts = Ts-0.0001
%%
sys_d = c2d(sys, Ts, 'zoh'); 
Ad = sys_d.A; 
Bd = sys_d.B; 
Cd = sys_d.C; 
sys_d_CL = ss(Ad-Bd*K_lqr, Bd*H_lqr, Cd, 0, Ts);

t_d = 0:Ts:10 ;
cmd_d = double(t_d > 1);
[y, t_d, x_d] = lsim(sys_d_CL, cmd_d, t_d) ;
u_d = H*cmd_d'-k*y ;

Question13 = figure ;
plot(t_d, u_d, t_d, y, t_d, cmd_d, t_d, (Cw_L*x_d')');
xlabel("Temps (s)");
ylabel("Amplitude");
legend("$u(t)$", "$y(t)$", "$w_c(L,t)$", "$w(L,t)$", 'Interpreter', 'latex');
title({'Simulation du retour d''états avec pré-gain','avec un bloqueur d''ordre 0'}) ;

if SAVE
    exportgraphics(Question13, './latex/Illustrations/Question13.pdf')
end

%% Question 14

sys_d = c2d(sys, 0.01, 'zoh'); 
Ad = sys_d.A; 
Bd = sys_d.B; 
Cd = sys_d.C; 
sys_d_CL = ss(Ad-Bd*K_lqr, Bd*H_lqr, Cd, 0, 0.01);

t_d = 0:0.01:10 ;
cmd_d = double(t_d > 1);
[y, t_d, x_d] = lsim(sys_d_CL, cmd_d, t_d) ;
u_d = H*cmd_d'-k*y ;

Question14 = figure ;
plot(t_d, u_d, t_d, y, t_d, cmd_d, t_d, (Cw_L*x_d')');
xlabel("Temps (s)");
ylabel("Amplitude");
legend("$u(t)$", "$y(t)$", "$w_c(L,t)$", "$w(L,t)$", 'Interpreter', 'latex');
title({'Simulation du retour d''états avec pré-gain','avec un bloqueur d''ordre 0'}) ;

if SAVE
    exportgraphics(Question14, './latex/Illustrations/Question14.pdf')
end


%% Question 15
Bp = [0; 0; 0; 0; 1/2*L^4-L^3+L; L^3-1/2*L^4; 1/4*L^4-2/3*L^3+1/2*L^2; 1/4*L^4-1/3*L^3];

q0 = -5 * double (t' >= 4);
sys_perturbations = ss(A-B*K_lqr, [B*H_lqr Bp], C, [0 0]);

%% Question 16
[y, t, x] = lsim(sys_perturbations, [cmd ; q0], t');
u = H_lqr*cmd'-(K_lqr*x')' ;
Question16 = figure ;
plot(t, u, t, y, t, cmd, t, (Cw_L*x')');
xlabel("Temps (s)");
ylabel("Amplitude");
legend("$u(t)$", "$y(t)$", "$w_c(L,t)$", "$w(L,t)$", 'Interpreter', 'latex');
title("Simulation du retour d'états avec pré-gain et perturbations") ;

if SAVE
    exportgraphics(Question16, './latex/Illustrations/Question16.pdf')
end

%% Question 17
eig_lqr = eig(sys_CL_lqr);
eig_4 = [eig_lqr ; -2];
A_aug = [A, zeros(8,1) ; -Cw_L, 0];
B_aug = [B; 0];
B_p_aug = [Bp; 0];
B_ref = [zeros(8,1); 1];

%% Question 18
K_aug = place(A_aug, B_aug, eig_4)

%% Question 19
sys_integral = ss(A_aug - B_aug*K_aug, [B_ref B_p_aug], [C, 0], [0 0]);

[y, t, x] = lsim(sys_integral, [cmd ; q0], t');
u = -(K_aug*x')' ;
Question19 = figure ;
plot(t, u, t, y, t, cmd, t, ([Cw_L 0]*x')');
xlabel("Temps (s)");
ylabel("Amplitude");
legend("$u(t)$", "$y(t)$", "$w_c(L,t)$", "$w(L,t)$", 'Interpreter', 'latex');
title("Simulation du retour d'états avec action intégrale et perturbations") ;
if SAVE
    exportgraphics(Question19, './latex/Illustrations/Question19.pdf')
end

%%
sys_aug = ss(A_aug, B_aug, [C, 0], 0);
for Ts = 0.01000:0.00001:0.01100
    if all(abs(eig(c2d(sys_aug,Ts,'zoh').A-c2d(sys_aug,Ts,'zoh').B*K_aug)) < 1)
        ;
    else
        break
    end 
end
format long
Ts = (Ts-0.00001)

Ts/5

%%
sys_aug_2 = ss(A_aug, [B_aug B_ref B_p_aug], [C 0], zeros(1,3));

sys_aug_d = c2d(sys_aug_2, Ts, 'zoh');

Ad2 = sys_aug_d.A;

Bd_u   = sys_aug_d.B(:,1);
Bd_ref = sys_aug_d.B(:,2);
Bd_p   = sys_aug_d.B(:,3);

sys_integral_d = ss(Ad2 - Bd_u*K_aug, [Bd_ref Bd_p], [C 0], [0 0], Ts);

% sys_integral_d = ss(sys_aug_d.A - sys_aug_d.B*K_aug, [B_ref B_p_aug], [C, 0], [0 0], Ts);

t_d = 0:Ts:10;
cmd_d = double (t_d >= 1) ;
q0_d = -5 * double(t_d >= 4);
[y_d, t_d, x_d] = lsim(sys_integral_d, [cmd_d ; q0_d], t_d);
u_d = -(K_aug*x_d')' ;
Question20 = figure ;
plot(t_d, u_d, t_d, y_d, t_d, cmd_d, t_d, ([Cw_L 0]*x_d')');
xlabel("Temps (s)");
ylabel("Amplitude");
legend("$u(t)$", "$y(t)$", "$w_c(L,t)$", "$w(L,t)$", 'Interpreter', 'latex');
title("Simulation du retour d'états avec action intégrale et perturbations") ;
