clc
clear all
close all
%%
%%%%%Valve_1%%%%

Kc0 = -220;
dy_s = 21;
t_init = 320;
u_init = 30;
dt = 0.002;

load step_test4
y = step_test4 (:,1);
n = length(y);
t = 0:dt:(n-1)*dt;
r = [149*ones(1,t_init/dt) 170*ones(1,n-t_init/dt)];

%%%%Low pass filter
n = length(y);
yf1 = zeros(1,n);
yf = zeros(1,n);
yff = zeros(1,n);
alpha = 0.001; % alpha=1 means no filtering
y_rev = zeros(1,n);
for k=1:n
    if (k==1)
        yf1(k) = y(k);
    elseif (k==2)
        yf1(:,k) = alpha*y(k) + (1-alpha)*yf1(:,k-1);
    else
        yf1(:,k) = alpha^2*y(k) + 2*(1-alpha)*yf1(:,k-1) - (1-alpha)^2*yf1(:,k-2);
    end
    y_rev(1,n-k+1) = yf1(:,k);
end


for k=1:n
    if (k==1)
        yf(k) = y_rev(k);
    elseif (k==2)
        yf(:,k) = alpha*y_rev(k) + (1-alpha)*yf(:,k-1);
    else
        yf(:,k) = alpha^2*y_rev(k) + 2*(1-alpha)*yf(:,k-1) - (1-alpha)^2*yf(:,k-2);
    end
    yff(1,n-k+1) = yf(:,k);
end

% End of low-pass filter

figure(1)
clf
plot(t,r,'r--','LineWidth',2)
hold on
plot(t,y,t,yff,'g','LineWidth',2)
xlabel('time(sec)');
ylabel ('Inlet pressure[kPa]');
legend ('Setpoint','Measurement','Filtered',3);


i_init = round(mean(find(t==t_init)));
y_plant = yff(i_init:end);
t_plant = t(i_init:end);
y_init = yff(i_init-100/dt);
yp = max(y_plant);
dy_p = abs(yp - y_init);
i_yp = find(y_plant==yp);
t_p = mean(t_plant(i_yp));
yu = min(y_plant(i_yp:10*i_yp));
dy_u = abs(yu - y_init);
i_yu = find(y_plant==yu);
t_u = mean(t_plant(i_yu));
y_inf = y_plant(end);
dy_inf = abs(y_inf - y_init);

Overshoot = abs((dy_p-dy_inf)/dy_inf);
D = Overshoot
Offset = abs((dy_s-dy_inf)/dy_inf);
B = Offset
A = 1.152*D^2 - 1.607*D +1;
r = 2*A/B;
K = 1/(Kc0 * B);
Tetha = t_p*(0.309 + 0.209*exp(-0.61*r));
Tau_c=Tetha;
tau1 = r*Tetha;
Kc = tau1/(K*2*Tetha)
tauI = min (tau1, 8*Tetha)

%%
%%%%%%%%%%%  Valve_2  %%%%%%%%
% clc
% clear all
% close all
% 
% 
% Kc0 = -250;
% dy_s = 7;
% t_init = 322.2;
% u_init = 18;
% dt = 0.6;
% 
% load Data
% y = Data (:,3);
% n = length(y);
% t = 0:dt:(n-1)*dt;
% r = [155*ones(1,t_init/dt) 162*ones(1,n-t_init/dt)];
% 
% %%Low pass filter
% n = length(y);
% yf1 = zeros(1,n);
% yf = zeros(1,n);
% yff = zeros(1,n);
% alpha = 0.25; % alpha=1 means no filtering
% y_rev = zeros(1,n);
% for k=1:n
%     if (k==1)
%         yf1(k) = y(k);
%     elseif (k==2)
%         yf1(:,k) = alpha*y(k) + (1-alpha)*yf1(:,k-1);
%     else
%         yf1(:,k) = alpha^2*y(k) + 2*(1-alpha)*yf1(:,k-1) - (1-alpha)^2*yf1(:,k-2);
%     end
%     y_rev(1,n-k+1) = yf1(:,k);
% end
% 
% 
% for k=1:n
%     if (k==1)
%         yf(k) = y_rev(k);
%     elseif (k==2)
%         yf(:,k) = alpha*y_rev(k) + (1-alpha)*yf(:,k-1);
%     else
%         yf(:,k) = alpha^2*y_rev(k) + 2*(1-alpha)*yf(:,k-1) - (1-alpha)^2*yf(:,k-2);
%     end
%     yff(1,n-k+1) = yf(:,k);
% end
% 
% % End of low-pass filter
% 
% 
% figure(2)
% clf
% plot(t,r,'r--','LineWidth',2)
% hold on
% plot(t,y,t,yff,'g','LineWidth',2)
% xlabel('time(sec)');
% ylabel ('Inlet pressure[kPa]');
% xlim ([0 650]);
% legend ('Setpoint','Measurement','Filtered',3);
% 
% i_init = round(mean(find(t==t_init)));
% y_plant = yff(i_init:end);
% t_plant = t(i_init:end);
% y_init = yff(i_init-120/dt);
% yp = max(y_plant);
% dy_p = abs(yp - y_init);
% i_yp = find(y_plant==yp);
% t_p = mean(t_plant(i_yp));
% yu = min(y_plant(i_yp:10*i_yp));
% dy_u = abs(yu - y_init);
% i_yu = find(y_plant==yu);
% t_u = mean(t_plant(i_yu));
% y_inf = y_plant(end);
% dy_inf = abs(y_inf - y_init);
% 
% Overshoot = abs((dy_p-dy_inf)/dy_inf);
% D = Overshoot
% Offset = abs((dy_s-dy_inf)/dy_inf);
% B = Offset
% A = 1.152*D^2 - 1.607*D +1;
% r = 2*A/B;
% K = 1/(Kc0 * B);
% Tetha = t_p*(0.309 + 0.209*exp(-0.61*r));
% Tau_c=Tetha;
% tau1 = r*Tetha;
% Kc = tau1/(K*2*Tetha)
% tauI = min (tau1, 8*Tetha)
% 
% 
