%Level control using 1 PI and two P controllers
%November/December 2017

clear;
close all;

%% Simulation parameters
t_sim = 200; %simulation time
num=4;       %case (1,2,3,4)-> To save results

%% MODEL
%To work with physical variables, we are using an interpreted Matlab
%function called SimpleTank_ODE  dh/dt=(qin-qout)/A. 
% This could be substituted with an integrator (1/s). It is left as it is
% in case future changes are required.

%Nominal values
qout0= 0.5;
qin0= 0.5;
h0=0.5;
h_set = 0.5; %Set-point for PI controller
p=1; %Area of the tank
theta=0; %Time delay in the tank

%% DISTURBANCES
% The disturbance is the inflow

sin_ON =1;             % add sinusoidal to nominal inflow
d1amplitude=0.05;
d1bias=0;
d1freq=2; %rad/s
d1phase=0; %rad

noise_ON=0;    % add noise
noise_power=1e-5;
ratelimit=10000; %to limit P controllers
%tfilter=0.2; %measurement filter

d2= 0.2; %step disturbance
td2=50;%20;  

d3= 0.2;
td3=100;%40; 

d4=0.05;
td4=150;%60;

d5=-0.9;
td5=205;%75;

%% PARAMETERS CONTROLLERS

%PI controller
tau_c = 3;              % PI controller tuning variable
%SIMC tuning
kprime=-1/p;
Kc=(1/kprime)*(1/(tau_c+theta));
tauI=4*(tau_c+theta);


%P controllers
f=20; % Factor to make P-controller gain higher than PI-controller gain
Kcmin=f*Kc; 
Kcmax=Kcmin; %both P-controllers have the same gain
offset=0.06;
Ymin=0.1+offset; % minimum level
Ymax=0.9-offset; % maximum level
Umax=1;   % minimum output (q_out,max)
Umin=0;   % maximum output (q_out,min)

%Saturation of input given u0
Uminsat=Umin-qout0;
Umaxsat=Umax-qout0;

%% SIMULATION

sim('levelcontrol_3PIctrl',t_sim) 

IAE_q=IAE_q.signals.values(end)
IAE=IAEmid.signals.values(end)
%qoutsumfin=qoutsum(end)
% %Save results
% name=strcat('Results/PI',num2str(num));
% save(name,'h','q_in','q_outmax','q_outmin','q_outmid','q_out','tim','hmin','hmax','hminmin','hmaxmax','hsp','IAE_q','IAE','derqout');

%% PLOT
Linewidth=1.3;
hmin=[0.1,0.1];
hmax=[0.9,0.9];
hminmin=[0,0];
hmaxmax=[1,1];
hsp=[0.5,0.5];
tim=[0,t_sim];


figure(1);
subplot(3,1,2)
plot(h.time,h.signals.values,'k','LineWidth',Linewidth)
title('Proposed PI-based controller')
hold on
plot(tim,hmin,'--','Color',[255/255 165/255 0/255])
plot(tim,hmax,'--','Color',[255/255 165/255 0/255])
plot(tim,hminmin,'--r')
plot(tim,hmaxmax,'--r')
plot(tim,hsp,'--','Color',[0 165/255 115/255])
hold off
%legend('h','h_{min}','h_{max}','h_{sp}'); 
%legend('h','h_{min}','h_{max}','h_{sp}'); 
ylim([-0.1 1.1]); grid on; grid minor; ylabel('Level [m]')

subplot(3,1,1)
plot(q_in.time,q_in.signals.values,'LineWidth',Linewidth)
title('Disturbance')
%hold on;
%plot(q_out.time,q_out.signals.values,'k')
%title('1 PI with estimator and 2 P controllers')
%legend('q_{in} [m^3/s]','q_{out} [m^3/s]')
%yticks(0:0.2:1);
%lgd=legend('q_{in}','q_{out}'); 
%lgd.FontSize=8;
%lgd.Location='northeast';
ylim([-0.1 1.1]); grid on; grid minor
%xlabel('time [s]');%ylabel('Flow [m^3/s]')
ylabel('Inflow [m^3/s]')
hold off

subplot(3,1,3)
plot(q_outmax.time,q_outmax.signals.values,'r','LineWidth',Linewidth)  %,'Color',[105/255 56/255 27/255])
hold on;
plot(q_outmid.time,q_outmid.signals.values,'Color', [50/255 205/255 50/255],'LineWidth',2) 
hold on;
plot(q_outmin.time,q_outmin.signals.values,'Color',[255/255 165/255 0/255],'LineWidth',Linewidth)
hold on;
plot(q_out.time,q_out.signals.values,':k','LineWidth',1.5)
hold on;
legend('q_{max,out}','q_{mid,out}','q_{min,out}','q_{out}','Location','southeast')
% a1=annotation('textarrow',[0.37 0.35],[0.23 0.23],'String','q_{out,max}');
% a2=annotation('textarrow',[0.57 0.57],[0.28 0.245],'String','q_{out,mid}');
% a3=annotation('textarrow',[0.69 0.66],[0.23 0.23],'String','q_{out,min}');
% a1.FontSize=10;
% a1.HeadLength=4;
% a1.HeadWidth=8;
% a2.FontSize=a1.FontSize;
% a3.FontSize=a1.FontSize;
% a2.HeadLength=a1.HeadLength;
% a3.HeadLength=a1.HeadLength;
% a2.HeadWidth=a1.HeadWidth;
% a3.HeadWidth=a1.HeadWidth;
% text(140,0.45,['q_{max,out}'],...
% 	'VerticalAlignment','bottom',...
% 	'HorizontalAlignment','center',...
% 	'FontSize',10)
ylim([-0.1 1.1]); grid on; grid minor
xlabel('time [s]');ylabel('Outflow [m^3/s]')


