clc
clear
close all

Po = 600000; %Pa - pressão do reservatorio, constante
M = 5; %kg - peso da massa movida + massa do pistão
D = 40e-3; %m - Diâmetro interno do cilindro
Aa = (pi*D^2)/4; %m^2 - Área transversal do cilindro
d = 1e-2 ;%m - Diâmetro do eixo acoplado ao pistão/barril
Ab = Aa - (pi*d^2)/4; %m^2 - Área transversal do compartimento à direita
L = 0.5; %m - Comprimento total interno do cilindro
VoA = Aa*L/2; % m^3 - Volume interno inicial A
VoB = Ab*L/2; % m^3 - Volume interno inicial B
R = 288; % Constante de gás do ar
To = 293.15;
rho0 =  Po/(R*To);% kgm3 - Densidade do gás dentro do reservatório
K3 = 50; % coeficiente de amortecimento viscoso do pistão
K2 = 5E-8; % Coeficiente que relaciona vazão mássica com posição normalizada do carretel
Omegans = 760; % Rad/s - frequencia natural do movimento do carretel
Zetas = 0.47; % Fator de amortecimento do movimento do carretel
 
alpha = Po^2*K2*(Aa/VoA + Ab/VoB)/(2*rho0*M);
beta = Po*(Aa^2/VoA + Ab^2/VoB)/(2*M);
gamma = K3/M;
epsilon = 2*Omegans*Zetas;
lambda = Omegans^2;
% 
A = [ 0      ,   1       , 0  ,  0    ,  0     ;
     -lambda ,  -epsilon , 0  ,  0    ,  0     ;
      0      ,   0       , 0  ,  1    ,  0     ;
      0      ,   0       , 0  ,  0    ,  1     ;
      alpha  ,   0       , 0  , -beta , -gamma ;];
%   
B = [ 0      ;
      lambda ;
      0      ;
      0      ;
      0      ;];
  
 C = [ 0 , 0 , 1 , 0 , 0 ];
%     
 D = 0;
% 
 Sistema = ss(A,B,C,D);
%%%%%%%%%%%%%%%%%%%%%%%%
%%% Polos, FT e Bode %%%
%%%%%%%%%%%%%%%%%%%%%%%%
% 
% Polos = pole(Sistema)
% FT = tf(Sistema)
%  
% figure()
% pzmap(Sistema)
% title('Mapa de polos do sistema','Fontsize',16)
% set(0, 'DefaultLineLineWidth', 3);
%  
 figure()
 set(0, 'DefaultLineLineWidth', 3);
 grid on
 bode(feedback(Sistema,1))
 title('Diagrama de Bode sistema em malha fechada','Fontsize',16)
% 
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%% Respostas do sistema %%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
tfinal = 4;
dt = 0.0001;
t = 0:dt:tfinal;
% 
% %Impulso%
% figure(10)
% imp = impulse(Sistema,t);
% plot(t,imp)
% title('Resposta ao impulso','Fontsize',16)
% ylabel('Posição da massa(m)','Fontsize',16)
% xlabel('Tempo(s)','Fontsize',16)
% legend('Posição','Fontsize',12)
% 
% %degrau%
% figure()
% deg = step(Sistema,t);
% plot(t,deg)
% title('Resposta ao degrau unitário','Fontsize',16)
% ylabel('Posição da massa(m)','Fontsize',16)
% xlabel('Tempo(s)','Fontsize',16)
% legend('Posição','Fontsize',12)
% 
% %Resposta a Impulso de 25ms de duração%
% 
% DurImp = [25, 50]; % Duração em ms
% Inten = [0.05, 0.1, 0.25];
% RespTemp = zeros(length(t),6);
% i = 1;
% for m = 1:3
%     for n = 1:2 
%         u1 = ones(1,DurImp(n))*Inten(m);
%         u2 = zeros(1,(length(t)-DurImp(n)));
%         uImp = [u1 u2];
%         RespTemp(:,i) = lsim(Sistema,uImp,t);
%         i = i + 1;
%     end
% end
% figure()
% plot(t,RespTemp)
% ylabel('Posição da massa(m)','Fontsize',16)
% xlabel('Tempo(s)','Fontsize',16)
% grid on
% title('Resposta temporal da posição da massa','Fontsize',16)
% legend('5%, 25ms','10%, 25ms','25%, 25ms','5%, 50ms','10%, 50ms','25%, 50ms','Fontsize',12)
% %x0=500;
% %y0=300;
% % width=1000;
% % height=450;
% % set(gcf,'position',[width,height])
% % set(0, 'DefaultLineLineWidth', 3);
% 
% %Resposta a entrada senoidal
% usen = 0.25*sin(60*t);
% RespSen = lsim(Sistema,usen,t);
% figure()
% plot(t,RespSen)
% ylabel('Posição da massa(m)','Fontsize',16)
% xlabel('Tempo(s)','Fontsize',16)
% grid on
% title('Resposta a entrada senoidal','Fontsize',16)
% legend('Posição','Entrada','Fontsize',12)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%% Matriz de transição %%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
 %Geração da Matriz
% k = 8; %numero de termos de e^(At)
% % 
% MatTrans = eye(length(A));
% Gam = eye(length(A));
% for i=1:k
%     MatTrans = MatTrans + ((A^i)*(dt^i))/factorial(i); %Matriz de transição
%     Gam = Gam +((A*dt)^i)/factorial(i+1); %Matriz Gamma (u considerado cte no intervalo de discretização)
% end
% disp("Matriz de transição")
% disp(MatTrans)
% disp("Termo forçante")
% disp(Gam)
% 
%  %Resposta a Impulso de 25ms de duração%
% nmax = length(t); %pontos simulados
% u0 = 0.1; %Entrada inicial
% x0 = [0; 0; 0; 0; 0]; %Condições iniciais
% xtrans1 = zeros(5,nmax);
% ytrans1 = zeros(1,nmax)*0.1;
% utrans = ones(1,nmax);
% xtrans1(:,1) = MatTrans*x0 + dt*Gam*B*u0; %x(1)
% ytrans1(:,1) = C*x0; %y(1)
% % 
% for n = 2:nmax
%      xtrans1(:,n) = MatTrans*xtrans1(:,n-1) + dt*Gam*B*utrans(:,n-1); %Resposta temporal discreta
%      ytrans1(:,n) = C*xtrans1(:,n-1); %Resposta observada
% end
% % 
% figure(10)
% plot(t,ytrans1)
% ylabel('Posição da massa(m)')
% xlabel('Tempo(s)')
% grid on
% title('Resposta a um degra de 10% na aberura da válvula de três vias')
% 
% Po = 600000; %Pa - pressão do reservatorio, constante
% M = 5; %kg - peso da massa movida + massa do pistão
% D = 40e-3; %m - Diâmetro interno do cilindro
% Aa = (pi*D^2)/4; %m^2 - Área transversal do cilindro
% d = 1e-2 ;%m - Diâmetro do eixo acoplado ao pistão/barril
% Ab = Aa - (pi*d^2)/4; %m^2 - Área transversal do compartimento à direita
% L = 0.5; %m - Comprimento total interno do cilindro
% VoA = Aa*L/2; % m^3 - Volume interno inicial A
% VoB = Ab*L/2; % m^3 - Volume interno inicial B
% R = 288; % Constante de gás do ar
% To = 293.15;
% rho0 =  Po/(R*To);% kgm3 - Densidade do gás dentro do reservatório
% K3 = 50; % coeficiente de amortecimento viscoso do pistão
% K2 = 5E-8; % Coeficiente que relaciona vazão mássica com posição normalizada do carretel
% Omegans = 760; % Rad/s - frequencia natural do movimento do carretel
% Zetas = 0.47; % Fator de amortecimento do movimento do carretel
% epsilon = 2*Omegans*Zetas;
% lambda = Omegans^2;
% 
% u = 0.1;
% [tode,xode] = ode45(@atuador, [0 1.5], [0; 0; 0; 0; 0; Po/2; Po/2]);
% 
% 
%%%Runge Kutta%%%
 
 h = 0.0001;
 x = zeros(7, length(t));
 x0 = [0; 0; 0; 0; 0; Po/2; Po/2];
 x(:,1) = x0;
 n = length(t)-1;
 
 
 for i = 1:n
     
     K1 = atuador( t(i)      , x(:,i)          );
     K2 = atuador( t(i) + h/2, x(:,i) + K1*h/2 );
     K3 = atuador( t(i) + h/2, x(:,i) + K2*h/2 );
     K4 = atuador( t(i) + h  , x(:,i) + K3*h   );
     x(:,i+1) = x(:,i) + (h/6)*( K1 + 2*K2 + 2*K3 + K4 );
     if x(1,i+1) > 0.25
         x(1,i+1) = 0.25;
     end
 end
 figure(11)
 plot(t,x(1,:))
 title('Resposta ao degrau - emulação sistema real');
 xlabel('t(s)');
 ylabel('Posição da massa (m)');
 set(0, 'DefaultLineLineWidth', 2)
 
 
 function dxdt = atuador(t,x)
 
 Po = 600000; %Pa - pressão do reservatorio, constante
 M = 5; %kg - peso da massa movida + massa do pistão
 D = 40e-3; %m - Diâmetro interno do cilindro
 Aa = (pi*D^2)/4; %m^2 - Área transversal do cilindro
 d = 1e-2 ;%m - Diâmetro do eixo acoplado ao pistão/barril
 Ab = Aa - (pi*d^2)/4; %m^2 - Área transversal do compartimento à direita
 L = 0.5; %m - Comprimento total interno do cilindro
 VoA = Aa*L/2; % m^3 - Volume interno inicial A
 VoB = Ab*L/2; % m^3 - Volume interno inicial B
 R = 288; % Constante de gás do ar
 To = 293.15;
 rho0 =  Po/(R*To);% kgm3 - Densidade do gás dentro do reservatório
 K3 = 50; % coeficiente de amortecimento viscoso do pistão
 K2 = 5E-8; % Coeficiente que relaciona vazão mássica com posição normalizada do carretel
 Omegans = 760; % Rad/s - frequencia natural do movimento do carretel
 Zetas = 0.47; % Fator de amortecimento do movimento do carretel
 epsilon = 2*Omegans*Zetas;
 lambda = Omegans^2;
 u = 0.1;
 
 FdelPponto = (((Po*K2*x(4))*(Po-x(6))/(rho0*(VoA+Aa*x(1)))-(x(6)*Aa*x(2))/(VoA+Aa*x(1)))*Aa)-(((Po*K2*x(4))*(-Po+x(7))/(rho0*(VoB+Ab*x(1)))+(x(7)*Ab*x(2))/(VoB+Ab*x(1)))*Ab);
 
 dxdt = [x(2); x(3); ((FdelPponto - K3*x(2))/M); x(5); (-lambda*x(4) - epsilon*x(5) + lambda*u); ((Po*K2*x(4))*(Po-x(6))/(rho0*(VoA+Aa*x(1)))-(x(6)*Aa*x(2))/(VoA+Aa*x(1))); ((Po*K2*x(4))*(-Po+x(7))/(rho0*(VoB+Ab*x(1)))+(x(7)*Ab*x(2))/(VoB+Ab*x(1)))];
 end