%PME 3481 2024
% Prof. Flávio Celso Trigo

% Código exemplo para seguidor de referência

% (ref. Franklin-Powell pg. 486 exemplo 7.14 e 7.15)


clear all;
close all;
clc;

% Sistema original

wo=1; % freq. natural
zeta=0; % sem amortecimento

A = [0 1; -wo^2 0]; B=[0;1]; C=[1 0]; D=[0];

eig(A)

sist_o=ss(A,B,C,D);

% Polos de MF desejados
p1=-2*wo;
p2=-3*wo;

% Ganho do controlador
K=place(A,B,[p1 p2]);

eig(A-B*K)

%Sistema controlado sem pre-alimentação

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

%Sistema controlado para seguidor de referência

Ar=A; Br=[0;1]; Cr=C; Dr=0;

AA=[Ar Br; Cr D]

BN = [zeros(2,1); 1]; % matriz lado direito da equação de Nx e Nu

N=AA\BN; Nx=N(1:2,1); Nu=N(3,1);

sist2 = ss(A-B*K, B*(Nu+K*Nx),C,D);


%Simulações
t=0:0.01:10;

%sistema original
[yo,t,x]=impulse(sist_o,t);
plot(t,yo, 'Linewidth', 4); grid; hold;


[y1,t,x]=impulse(sist1,t);
[y1s,t,x]=step(sist1,t);
plot(t,y1, 'Linewidth', 4, 'k', t,y1s, 'Linewidth', 4, 'g');
[y2s,t,x]=step(sist2,t);

%simulação a uma entrada senoide e a uma entrada rampa (ainda sem ruídos)

Useno = 1+sin(1*wo*t); %referência senoidal com nível CC
Urampa = t; Urampa=[zeros(200,1) ; t(1:end-200)]; %referência em rampa
Uretang = [zeros(200,1); ones(200,1); zeros(200,1); ones(200,1); zeros(201,1)];
X0=[4;0];
[yUseno,t,xU]=lsim(sist2,Useno,t,X0);
[yUrampa,t,xUrampa] = lsim(sist2,Urampa,t,X0);
[yUretang,t,xUretang]= lsim(sist2,Uretang,t,X0);
hold off;
plot(t,y2s, 'Linewidth', 4, 'r');
set(gca,'FontSize', 14);

figure(2);
plot(t,Urampa, 'Linewidth', 3, 'c', t, yUrampa, 'Linewidth', 3, 'r');
set(gca,'FontSize', 14);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%
%
%%Simulação do sistema com os polos escolhidos sujeito a entradas de referência variável e de distúrbio
%
%disp('Simulação para seguimento de referência e distúrbio')
%
%% Matrizes do novo sistema com entradas de referência e distúrbio:
%
%% A matriz do sistema em malha aberta continua sendo A, bem como o ganho K já
%% calculado com os polos das linhas 19 e 20. Aqui vou chamá-lo de Gk

Gk=K;

%% Referência a seguir:
% 1) rampa unitária que corresponde à matriz de estados
Ar=[0 1; 0 0]; % correto isso, corresponde a eq. diferencial
% d2x/dt2=delta(t) ( delta de Dirac).
Aw=zeros(2); % inicialmente, não há distúrbio


 % 2) Novo sistema completo com seguidor de referência e distúrbio
B1 = B; B2=B;



C1=[1 0];

F=A-B2*K
F1=inv(F);
F2=[B1 A-Ar];
% 3) ganho de pré-alimentação para seguir referência

Gr=inv(C1*F1*B2)*C1*F1*F2;
A0=[Aw zeros(2); zeros(2) Ar];
Ay=[B1 B2*Gk]-B2*Gr;

Ay=[Ay [0;0]];
AT=[A-B2*Gk Ay; zeros(4,2) A0];

xT0=[1;0;1;0;1;0];

% 4) Sistema completo
%sist_sigref=ss(AT, xT0, AT, xT0)
sist_sigref=ss(AT,xT0,[C1 0 0 0 0],D)

%Gr = inv(C*inv(A-B2*Gk)*B2)*C*inv(A-B2*Gk)*(A-B2*Gk);

% 5) Simulação entrada rampa

[ysigref, t, xsigref]=lsim(sist_sigref,t,t);

figure(3)
set(gca,'FontSize', 16);
plot(t,ysigref,'Linewidth',3,'g-', t, t , 'Linewidth',4, 'r-')
set(gca,'FontSize', 14);
%