%% [BE] Digital control of cantilever beam
% Justin Bos
% 
% Wissal Guarni
% 
% Nolan Reynier Nomer
% 
% Aleksander Taban

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;
%%
A = [zeros(4), inv(E)*D ;
    -inv(E)*(D'), zeros(4) ] ;
phi_L = phi(L) ;
B = [zeros(4,1) ; -inv(E)*phi_L] ;
C = [zeros(1,4) , -phi_L'] ;
%%
Ts1 = 0.01 ;
Ts2 = 0.02 ;
Ts3 = 0.04 ;

eig_cont = eig(A);
sys = ss(A, B, C, 0) ;
figure;
subplot(1,3,1) ;
plot(eig_cont, 'x');
hold off;

subplot(1,3,2);
eig_tustin1 = eig(c2d(sys, Ts1, 'tustin')) ;
eig_tustin2 = eig(c2d(sys, Ts2, 'tustin')) ;
eig_tustin3 = eig(c2d(sys, Ts3, 'tustin')) ;
plot(eig_tustin1,'x');
hold on;
plot(eig_tustin2,'x');
plot(eig_tustin3,'x');
hold off;

subplot(1,3,3);
eig_zoh1 = eig(c2d(sys, Ts1, 'zoh')) ;
sys_zoh1 = c2d(sys, Ts1, 'zoh');
eig_zoh2 = eig(c2d(sys, Ts2, 'zoh')) ;
eig_zoh3 = eig(c2d(sys, Ts3, 'zoh')) ;
plot(eig_zoh1,'x');
hold on;
pzmap(sys_zoh1);
plot(eig_zoh3,'x');
hold off;
%% 
% Conlsuion : On en conclut qqc.

figure;
t = 0:0.01:10;
u = double (t >= 1) ;
lsim(sys,u, t);
title();
legend();
xlabel();
ylabel();
%% 
% 

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 = [] ;
for i = 0.01:0.01:10
    TransientTime(end+1) = stepinfo(feedback(sys, i)).TransientTime ;
end
[minimum, min_index] = min(TransientTime)
k = min_index * 0.01;
lsim(feedback(sys,k), u, t) ;
H = -3
%%
sys_CL = feedback(sys,k);
[y, tOut] = lsim(sys_CL, -3.*u, t);
% plot(t, Cw(L)*y, t, u)
eig_cont_CL = eig(sys_CL);
figure;
subplot(1,2,1) ;
plot(eig_cont_CL, 'x');
hold off;

subplot(1,2,2);
eig_zoh1 = eig(c2d(sys_CL, Ts1, 'zoh')) ;
eig_zoh2 = eig(c2d(sys_CL, Ts2, 'zoh')) ;
eig_zoh3 = eig(c2d(sys_CL, Ts3, 'zoh')) ;
plot(eig_zoh1,'x');
hold on;
plot(eig_zoh3,'x');
hold off;
%%
sys_CL_Ts = c2d(sys_CL, 10, 'zoh');
figure
step(sys_CL_Ts)
%%
K = lqr(sys, eye(size(A)), 1)
sys_CL_lqr = ss(A-B*K, B*H, C, 0);
lsim(sys_CL_lqr, u, t);