% Prof. Flávio Celso Trigo
% 18/04/2024
% CONTROLE E APLICAÇÕES

%Pêndulo invertido de Furuta: síntese de reguladores por alocação de polos e LQR
clc();
clear all;
close all;


%Parãmetros

J1 = 0.05; l1 = 0.2; M2 = 0.15; l2 = 0.5;
J2 = M2*l2^2/3; Kphi = 0.008; N = 15; R = 2.5; g = 9.8;

% Parâmetros calculados auxiliares
a = J1 + M2*l1^2; b = 1/2*M2*l1*l2; c = J2;
d = 1/2*M2*g*l2; e = N*Kphi/R; f = N^2*Kphi^2/R;
delta = a*c-b^2;

% Sistema linearizado
 % Matrizes do sistema linearizado
A = [0 0 1 0; 0 0 0 1; 0 -b*d/delta 0 0; 0 a*d/delta 0 0];
B = [0; 0; c/delta; -b/delta];
C = [1 0 0 0; 0 1 0 0];
D = [0;0];

%Montagem do sistema em espaço de estados

disp("1) verificação de controlabilidade e observabilidade");
CTR = rank(ctrb(A,B));
disp('Posto de contr.='), disp(CTR);
OBSERV = rank(obsv(A,C));
display('Posto de observ.='), disp(OBSERV);

disp("2) Verificação de estabilidade: autovalores da matriz A");
disp("autovalores de A"), disp(eig(A));

disp("3) Projeto de controlador por alocação de pólos");

polos1=[-7.0+3*i; -7.0-3*i; -6.1-2*i; -6.1+2*i];
polos2=[-7+3*i; -7-3*i; -0.5; -0.6];
polos3=[-7; -8; -9; -10];

disp(" Matriz de ganhos de controle");
K = place(A,B,polos2)
disp("autovalores de K")
disp("autovalores de A-BK, polos de malha fechada");
eig(A-B*K)

disp("4) Montagem do sistema controlado em espaço de estados")

sist1=ss(A-B*K,B,C,D)

disp("5) Simulação de uma perturbação impulsiva");

[y,t,x]=impulse(sist1);

figure(1);
plot(t,y(:,1)*180/pi,t,y(:,2)*180/pi);
xlabel('tempo (s)');
ylabel('angulos (graus)');
legend('base', 'pêndulo'); grid;title("Controle por alocação de polos")
%ht = text (0.4, 20.0, "Pêndulo invertido", "fontsize", 20);
%set (ht, "color", "red");
%
disp("6) Controle LQR");

disp("Matrizes Q e R");

Q = diag([1000, 1000, 10, 10])
R = 1e-1;

disp("Sistema original não controlado")
sist_orig=ss(A,B,C,D)

disp("Estrutura do controlador LQR: lei de controle u = -B*G")

[G, X, L]=lqr(sist_orig,Q,R)

disp("Montagem do sistema controlado por LQR")

sist_lqr=ss(A-B*G,B,C,D);

disp("simulação do sistema em MF com LQR")

[ylqr,t,xlqr]=impulse(sist_lqr);

figure(2);
plot(t,ylqr(:,1)*180/pi,t,ylqr(:,2)*180/pi);
xlabel('tempo (s)');
ylabel('angulos (graus)');
legend('base', 'pêndulo'); grid;title("Controle LQR")