%
% Feedback Control Simulation
%
% Process: underdamped second order
% Controller: PID
%
% Bob Newell, February 1996
%
%----------------------------------------------------------
%
% Process Parameters
%
gain = 25;                % gain (vector)
tau1 = 20;                % smaller time constant (vector)
tau2 = 150;               % larger time constant (vector)
dt = 1;                   % sampling interval (scalar)
%
yo = 200;                 % initial output value
uo = 2;                   % initial input value
%
%----------------------------------------------------------
%
% Controller Parameters
%     u = uo + kp*e + ki*integral(e) + kd*derivative(e)
%
kp = [ 0.4 0.4 0.4 ];     % proportional gain (vector) 
ki = [ 0 0.005 0.005 ];   % integral gain (vector)
kd = [ 0 0 2.5 ];         % derivative gain (vector)
%
%----------------------------------------------------------
%
% Define the disturbance
%
ydstep = 5;               % size of setpoint change
dstep = 0;                % size of step load change
%----------------------------------------------------------
%
% determine simulations to run
%
n = length( gain );       % number of controllers
m = length( kp );         % number of processes
l = fix(2*max(tau2)/dt);  % length of simulation
%
yyy = [];
uuu = [];
%
for i = 1:n,
   for j = 1:m,
%
% initialise simulation   
      y = yo;             % initial output
      yd = yo + ydstep;   % setpoint
      yhat1 = 0;          % initial state 1
      yhat2 = 0;          % initial state 2
      u = uo;             % initial input
%
      esum = 0;           % initial integral error sum
      laste = 0;          % initial last error
%
      yy = [];
      uu = [];
%
      for k = 1:l,
%
%        controller
         e = yd - y;
         esum = esum + e * dt;
         dedt = ( e - laste ) / dt;
         laste = e;
         u = uo + kp(j) * e + ki(j) * esum + kd(j) * dedt ;
         uu = [ uu; u ];
%
%        process
         uhat = u - uo;
         yhat1 = yhat1 * ( tau1(i) - dt ) / tau1(i) + ...
                      ( uhat + dstep ) * dt / tau1(i);
         yhat2 = yhat2 * ( tau2(i) - dt ) / tau2(i) + ...
                      yhat1 * gain(i) * dt / tau2(i);
         y = yhat2 + yo;
         yy = [ yy; y ];
%
      end;
%
      yyy = [ yyy yy ];
	  uuu = [ uuu uu ];
%
   end;
end;
%
figure
plot( dt*(1:k)', uuu )
xlabel( 'Time' )
ylabel( 'Inputs' )
%
figure
plot( dt*(1:k)', yyy )
xlabel( 'Time' )
ylabel( 'Outputs' )
%
%----------------------------------------------------------
% the end
