%Her er den ulinære trykktankmodellen med PI-regulatoren

function yp=deriP(t,y,data)

%Regulatorparametere
Kc2=-83;
Kc4=-6.5e-5;

tau_Kc2=75;
tau_Kc4=25;
tau_u2=0.5;
tau_u4=0.5;

Pl=y(2)+data.rho*data.g*y(1);
eH=data.y0(1)-y(1);
eP=data.y0(2)-y(2);

%Pådrag
U2=data.U20+Kc2*eH+Kc2/tau_Kc2*y(3);
U4=data.U40+Kc4*eP+Kc4/tau_Kc4*y(4);
U2 = max(U2,0);
U2 = min(U2,1);
U4 = max(U4,0);
U4 = min(U4,1);

%Strømning gjennom ventilene
q2=data.Cv2*y(5)*sqrt(Pl-data.P0);
n4=data.Cv4*y(6)*sqrt(y(2)^2-data.P0^2);

%diff ligningene, det er lagt til en ifløkke for å forhindre at væskenivået går
%over 0,6 meter. Det burde egentlig legges til flere løkker for å hinder nivået å
%gå under 0 meter, og å forhindre for høyt/lavt trykk.

yp=zeros(6,1);
if y(1)>=0.6
    yp(1)=0;
    yp(2)=(data.R*data.T*(data.n3-n4)+y(2)*(data.q1-q2))/(data.Vtot-data.A*y(1));
    yp(3)=data.y0(1)-y(1);
    yp(4)=data.y0(2)-y(2);
    yp(5)=1/tau_u2*(U2-y(5));
    yp(6)=1/tau_u4*(U4-y(6));
else
yp(1)=(data.q1-q2)/data.A;
yp(2)=(data.R*data.T*(data.n3-n4)+y(2)*(data.q1-q2))/(data.Vtot-data.A*y(1));
yp(3)=data.y0(1)-y(1);
yp(4)=data.y0(2)-y(2);
yp(5)=1/tau_u2*(U2-y(5));
yp(6)=1/tau_u4*(U4-y(6));
end
yp=yp(:);

