

function [y,u,Duc,t]=PIDVeImp(Kc,tauI,tauD,tauf,num,den,deltat,r,bs)
%tauf=beta*tauD;
%deltat is the sampling interval
%making the plant model easier for calculation of output
%r is the set-point signal
%implemention with proportional control on feedback error
Umax=bs(1);
Umin=bs(2);
Dumax=bs(3);
Dumin=bs(4);
[Ap,Bp,Cp,Dp]=tf2ss(num,den);
%check the plant model order
[m1,n1]=size(Cp);
[n1,n_in]=size(Bp);
%specify plant initial condition
xm=zeros(n1,1);  %we use zeros here
Nsim=length(r);  %number of simulation samples
%simulation begins
uDPast=0;  % sample at -deltat
uPast=0;  % you can specify this equal to the actual control signal 
yCurrent=0;
yPast=0;
rPast=0;
rCurrent=r(1);  
for kk=1:Nsim-1;
%calculate control signal at the current sample    
 uDCurrent=tauf/(tauf+deltat)*uDPast+(Kc*tauD)/(tauf+deltat)*(yCurrent-yPast);
 uCurrent=uPast+Kc*(rCurrent-yCurrent-rPast+yPast)+(Kc*deltat)/tauI*(rCurrent-yCurrent)-uDCurrent+uDPast;
%anti-windup on rate of change
 Du=(uCurrent-uPast)/deltat;
 if (Du>Dumax)  uCurrent=uPast+Dumax*deltat; Du=Dumax;end
 if (Du<Dumin) uCurrent=uPast+Dumin*deltat;Du=Dumin; end
%anti-windup on amplitude 
    if (uCurrent>Umax) uCurrent=Umax;end
    if (uCurrent<Umin) uCurrent=Umin; end
    
 %past signals are updated in preparation for the next sample 
 uDPast=uDCurrent;
 uPast=uCurrent;
 yPast=yCurrent;
 rPast=rCurrent;
  %   %store the signals for plotting
    u(kk)=uCurrent;              
    y(kk)=yCurrent;
    Duc(kk)=Du;
 %send the current control signal to plant, if you are implementing it 
 %%%%
 %if you are in simulation, then calculate the plant output
 %%%%%%
    xm=xm+(Ap*xm+Bp*uCurrent)*deltat;      
    ym=Cp*xm;  
 %%%%%%%%%%%%%
 %updating the measurement and set-point signals 
 yCurrent=ym;
 rCurrent=r(kk+1);     
   
 
 %ready for next sample of control calculation                             
end
t=0:deltat:(Nsim-2)*deltat;
