% **************************************************************************
% CASCADE-FEEDFORWARD CONTROL SIMULATOR 
% COPYRIGHT (C) 1994, T.E. MARLIN
%
% FILENAME : CFFSIM.M
% WRITTEN BY : F.BOUDREAU
% VERSION : 2.0
% LAST UPDATED : NOVEMBER 1994
% DESCRIPTION OF FILE :
%    1. INITIALIZE VECTORS.
%    2. CALCULATE CONSTANTS FOR TRANSFER FUNCTIONS IN DISCRETE FORM.
%       NOTE THAT TRANSFER FUNCTIONS ARE DISCRETIZED BY FIRST
%       MULTIPLYING BY A ZERO-ORDER HOLD, AND THEN EXPRESSING IN
%       Z-TRANFORMS.  DEAD TIMES ARE ROUND OFF TO THE NEAREST INTEGER.
%    3. GENERATE MEASUREMENT NOISE.
%    4. PRECALCULATE DISTURBANCES AND FEEDFORWARD CONTROL ACTIONS.
%    5. EXECUTE SIMULATION. THERE ARE TWO SIMULATION LOOPS; ONE FOR
%       PRC2 MODE AND THE OTHER FOR ALL OTHER CASES (SEE COMMENTS IN
%       'SIMULATION LOOPS' SECTION).
%    5. CALCULATE PERFORMANCE INDICES.
%    6. ADD OFFSETS TO VARIABLES THAT WILL BE PLOTTED (THE SIMULATION
%       IS IN DEVIATION VARIABLES).
% ALL FILES IN THIS PACKAGE :
%    CFF     .M  - MAIN MENU AND ENTRY POINT
%    CFFDAT  .M  - EXTERNAL DATA FILE
%    CFFGRAPH.M  - PLOTS SIMULATION RESULTS
%    CFFM1   .M  - MENU FOR PROCESS T.F. EDITING
%    CFFM2   .M  - MENU FOR PID TUNING
%    CFFM3   .M  - MENU FOR DISTURBANCE T.F. EDITING
%    CFFMFF  .M  - FEEDFORWARD CONTROLLER MENU
%    CFFMSIM .M  - SIMULATION OPTIONS MENU
%    CFFNAME .M  - MENU FOR VARIABLES NAMES, INITIAL VALUES, BOUNDS
%    CFFSIM  .M  - EXECUTES SIMULATION
% **************************************************************************


% FLAG THAT INDICATES AT LEAST ONE SIMULATION HAS BEEN RUN:
EXECUTION = 1;

% IF CONTROLLER MODE IS SET TO PRC=2 AND CASCADE IS OFF, RESET TO PRC=0:
if CASCADE == 0 & PRC == 2
   PRC = 0;
end

% INDICATES TO THE USER THAT THE SIMULATION IS RUNNING
% (ON THE GRAPH SCREEN):
%clg;
%(0.35,0.5,'SIMULATION IN PROCESS','sc');

disp('    SIMULATION IN PROGRESS')

% TIME VECTOR PADDED WITH ZEROS AT THE BEGINNING:
t = [tstart.*ones(1,nsup) tstart:tstep:tend];

% INDEX VECTOR FOR SIMULATION LOOP, ALSO TO BE USED LATER FOR PLOTTING:
% ind = nsup+1:ntot;            %  old statement
  ind = nsup+1:max(size(t));    %  ensures that ntot and t have same
                                %  size regardless of of roundoff

% INITIALIZE VARIABLES
% --------------------
E1 = zeros(ntot,1);      % INPUT TO PRIMARY PID CONTROLLER
C1 = zeros(ntot,1);      % OUTPUT OF PRIMARY PID CONTROLLER
SP1 = zeros(ntot,1);     % SET POINT TO THE PRIMARY CONTROLLER
SP2 = zeros(ntot,1);     % SET-POINT TO THE SECONDARY CONTROLLER
E2 = zeros(ntot,1);      % INPUT TO THE SECONDARY CONTROLLER
C2 = zeros(ntot,1);      % OUTPUT OF THE SECONDARY CONTROLLER
X2 = zeros(ntot+1,1);    % OUTPUT THE INNER PROCESS G2, NOISE-FREE
Y2 = zeros(ntot,1);      % X2 + INNER DISTURBANCE GD2(S)*D2(S)
Y2m = zeros(ntot,1);     % Y2 + MEASUREMENT NOISE
X1 = zeros(ntot+1,1);    % OUTPUT OF OUTER PROCESS G1, NOISE-FREE
Y1 = zeros(ntot,1);      % X1 + OUTER DISTURBANCE GD1(S)*D1(S)
Y1m = zeros(ntot,1);     % Y1 + MEASUREMENT NOISE
Cff = zeros(ntot,1);     % OUTPUT OF FEEDFORWARD CONTROLLER
D1 = zeros(ntot,1);      % DISTURBANCE IN PRIMARY LOOP (MEASURED FOR FF)
D1m = zeros(ntot,1);     % MEASURED DISTURBANCE D1
D2 = zeros(ntot,1);      % DISTURBANCE IN SECONDARY LOOP 
N1 = zeros(ntot,1);      % GD1(S)*D1(S)
N2 = zeros(ntot,1);      % GD2(S)*D2(S)
NY1 = zeros(ntot,1);     % MEASUREMENT NOISE ON Y1
NY2 = zeros(ntot,1);     % MEASUREMENT NOISE ON Y2
ND1 = zeros(ntot,1);     % MEASUREMENT NOISE ON D1

% CALCULATE CONSTANTS FOR PROCESSES IN DISCRETE FORM
% --------------------------------------------------
% E.G. COEFFICIENTS FOR PRIMARY PROCESS
% Y1(k)+a1(i)Y1(k-1)+a2(i)Y1(k-2)=b1(i)C1(k-1)+b2(i)C1(k-2)
for i = 1:2
   if T1(i) == 0.0;                  % PERFORM THIS CHECK SO THAT 
      temp1(i) = 0.0;                % EXP(-INFINITY) IS GIVEN THE
   else                              % VALUE ZERO.
      temp1(i) = exp(-tstep/T1(i));
   end
   if T2(i) == 0.0;
      temp2(i) = 0.0;
   else
      temp2(i) = exp(-tstep/T2(i));
   end
   if T1(i) == T2(i)        %note: T1=T2 also imply temp1=temp2
      if T1(i) == 0         % CASE 1: T1=T2=0
         a1(i) = 0;
         a2(i) = 0;
         b1(i) = K(i)*(TA(i)/tstep+1);
         b2(i) = -K(i)*TA(i)/tstep;
      else                  % CASE 2: T1=T2 but not zero
         temp = (TA(i)-T1(i))/T1(i)^2*tstep;
         a1(i) = -2*temp1(i);
         a2(i) = temp1(i)^2;
         b1(i) = K(i)*(1+(temp-1)*temp1(i));
         b2(i) = K(i)*temp1(i)*(temp1(i)-1-temp);
      end
   else                     % CASE 3: T1 not equal to T2
      a1(i) = -temp1(i)-temp2(i);  
      a2(i) = temp1(i)*temp2(i);  
      b1(i) = K(i)*(1+(TA(i)-T1(i))/(T1(i)-T2(i))*temp1(i)...
                    +(T2(i)-TA(i))/(T1(i)-T2(i))*temp2(i));
      b2(i) = K(i)*(a2(i)+(TA(i)-T1(i))/(T1(i)-T2(i))*temp2(i)...
                    +(T2(i)-TA(i))/(T1(i)-T2(i))*temp1(i));      
   end
end

% CALCULATE CONSTANTS FOR DISTURBANCE PROCESSES IN DISCRETE FORM
% --------------------------------------------------------------
% E.G. COEFFICIENTS FOR PRIMARY DISTURBANCE: 
% N1(k)+a1d(i)N1(k-1)+a2d(i)N1(k-2)=b1d(i)D1(k-1)+b2d(i)D1(k-2)
for i=1:2
   if T1d(i)==0.0;                   % PERFORM THIS CHECK SO THAT
      temp1(i) = 0.0;                % EXP(-INFINITY) IS GIVEN THE
   else                              % VALUE ZERO.
      temp1(i) = exp(-tstep/T1d(i));
   end
   if T2d(i)==0.0;
      temp2(i) = 0.0;
   else
      temp2(i) = exp(-tstep/T2d(i));
   end
   if T1d(i) == T2d(i)
      if T1d(i) == 0
         a1d(i) = 0;
         a2d(i) = 0;
         b1d(i) = Kd(i)*(TAd(i)/tstep+1);
         b2d(i) = -Kd(i)*TAd(i)/tstep;
      else
         temp = (TAd(i)-T1d(i))/T1d(i)^2*tstep;
         a1d(i) = -2*temp1(i);
         a2d(i) = temp1(i)^2;
         b1d(i) = Kd(i)*(1+(temp-1)*temp1(i));
         b2d(i) = Kd(i)*temp1(i)*(temp1(i)-1-temp);
      end
   else
      a1d(i) = -temp1(i)-temp2(i);
      a2d(i) = temp1(i)*temp2(i);
      b1d(i) = Kd(i)*(1+(TAd(i)-T1d(i))/(T1d(i)-T2d(i))*temp1(i)...
                      +(T2d(i)-TAd(i))/(T1d(i)-T2d(i))*temp2(i));
      b2d(i) = Kd(i)*(a2d(i)+(TAd(i)-T1d(i))/(T1d(i)-T2d(i))*temp2(i)...
                      +(T2d(i)-TAd(i))/(T1d(i)-T2d(i))*temp1(i));
   end
end
% if temp(1)=0 b1d = Kd*(1+(T2d-TAd)/(-T2d)*temp2)

% CALCULATE COEFFICIENTS FOR CONTROLLERS IN DISCRETE FORM
% -------------------------------------------------------
% E.G. FOR PRIMARY PID:
% C1(k)-C1(k-1) = KC(1)*(b1c(1)E1(k)-E1(k-1)-TD(1)/tstep(...))
% FOR FEEDFORWARD:
% Cff(k)+a1ff*Cff(k-1)=b1ff*D1m(k-Dffd)+b2ff*D1m(k-1-Dffd)
%
% PID's:
for i=1:2          % IF INTEGRAL TIME CONST = 0, TURN OFF INTEGRAL MODE
  if TI(i) == 0
     b1c(i) = 1;
  else
     b1c(i) = 1+tstep/TI(i);
  end
end
% Feedforward:

   a1ff =      (Tlg/tstep    ) / (Tlg/tstep + 1);
   b1ff =  Kff*(Tld/tstep + 1) / (Tlg/tstep + 1);
   b2ff = -Kff*(Tld/tstep    ) / (Tlg/tstep + 1);


% GENERATE WHITE NOISE FOR THE MEASUREMENT ERRORS
% -----------------------------------------------
%   "randn" evaluates normally distributed random variable
if NOISE == 1
   % FOR NOISE ON PRIMARY OUTPUT MEASUREMENT:
   randn('seed',SeedY1);
   A1 = randn(ntot,1).*sqrt(SaY1);
   % FOR NOISE ON SECONDARY OUTPUT MEASUREMENT:
   randn('seed',SeedY2);
   A2 = randn(ntot,1).*sqrt(SaY2);
   % FOR NOISE ON FEEDFORWARD DISTURBANCE MEASUREMENT:
   randn('seed',SeedD1);
   AD1 = randn(ntot,1).*sqrt(SaD1);
else
   A1 = zeros(ntot,1);
   A2 = zeros(ntot,1);
   AD1 = zeros(ntot,1);
end

% CALCULATE DISTURBANCES AND FF CONTROL ACTIONS BEFORE MAIN SIMULATION LOOP
% -------------------------------------------------------------------------
Dts = Dtstep./100.*(tend-tstart);   % REAL TIME TO INTRODUCE DISTURBANCES
SP1ts = SPtstep/100*(tend-tstart);  % REAL TIME TO INTRODUCE S-P STEPS
for k = ind
   % INTRODUCE STEPS IN DISTURBANCES AT APPROPRIATE TIME:
   if (t(k) >= Dts(1) & DISTURBANCE == 1)
      D1(k) = Ddelta(1);
   end
   if (t(k) >= Dts(2) & DISTURBANCE == 1)
      D2(k) = Ddelta(2);
   end
   % OUTER LOOP DISTURBANCE:
   N1(k) = - a1d(1)*N1(k-1) - a2d(1)*N1(k-2) + ...
         b1d(1)*D1(k-1-DDd(1)) + b2d(1)*D1(k-2-DDd(1));
   % INNER LOOP DISTURBANCE:
   N2(k) = - a1d(2)*N2(k-1) - a2d(2)*N2(k-2) + ...
         b1d(2)*D2(k-1-DDd(2)) + b2d(2)*D2(k-2-DDd(2));
   % MEASUREMENT ERROR ON THE OUTER LOOP OUTPUT Y1:
   NY1(k) = dY1(1)*NY1(k-1) + dY1(2)*NY1(k-2) + A1(k);
   % MEASUREMENT ERROR ON THE INNER LOOP OUTPUT Y2:
   NY2(k) = dY2(1)*NY2(k-1) + dY2(2)*NY2(k-2) + A2(k);
   % MEASUREMENT ERROR ON THE OUTER LOOP DISTURBANCE:
   ND1(k) = dD1(1)*ND1(k-1) + dD1(2)*ND1(k-2) + AD1(k);
   % CALCULATE FF CONTROL ACTION:
   % * NOTE THAT FF IS AUTOMATICALLY TURNED OFF FOR PRC MODES
   if PRC == 0
      D1m(k) = D1(k)+ND1(k);
      Cff(k) = a1ff*Cff(k-1)+b1ff*D1m(k-Dffd)+b2ff*D1m(k-1-Dffd);      
   else
      D1m(k) = D1(k) + ND1(k);
      Cff(k) = 0;                      % no FF when PRC
   end
end

% SIMULATION LOOPS
% ----------------
% Note: There are 2 simulation loops, one for PRC = 2 and
% one for all other cases; some code is thus duplicated but
% simulation should run faster in PRC2 mode. Also this avoids a
% couple more cases to handle in the other (main) simulation loop.

% PROCESS REACTION CURVE INDEX:
% PRC = 0 : NORMAL SIMULATION, NO PRC (CASCADE OR SINGLE-LOOP)
% PRC = 1 : PRC FOR PRIMARY CONTROLLER (CASCADE OR SINGLE-LOOP)
% PRC = 2 : PRC FOR SECONDARY CONTROLLER, ALLOWED ONLY IN CASCADE MODE

if PRC == 2   % CALCULATE PRC FOR INNER LOOP
   for k = ind                  
      if t(k) >= SP1ts
         C2(k) = SP_PRC(2);
      end
      % INNER PROCESS, NOISE FREE PROCESS OUTPUT:
      X2(k+1) = - a1(2)*X2(k) - a2(2)*X2(k-1) + ...
                b1(2)*C2(k-DD(2)) + b2(2)*C2(k-1-DD(2)); 
      % ADD DISTURBANCE TO OUTPUT X2:
      Y2(k) = X2(k) + N2(k);
      % ADD MEASUREMENT NOISE:
      Y2m(k) = Y2(k) + NY2(k);
      X1(k+1) = -a1(1)*X1(k) - a2(1)*X1(k-1) + ...
                b1(1)*Y2(k-DD(1)) + b2(1)*Y2(k-1-DD(1));
      Y1(k) = X1(k) + N1(k);
      Y1m(k) = Y1(k) + NY1(k);
   end % for
else   % MODE PRC1 OR NORMAL SIMULATION (WITH CASCADE ON OR OFF)
   for k = ind         % loop in time indices
      if t(k) >= SP1ts
         SP1(k) = SPdelta;
      end
      % ADD DISTURBANCE TO OUTER PROCESS OUTPUT:
      Y1(k) = X1(k) + N1(k);
      % ADD MEASUREMENT NOISE:
      Y1m(k) = Y1(k) + NY1(k);
      if PRC == 0
         % OUTER CONTROLLER INPUT:
         E1(k) = SP1(k) - Y1m(k);
         % OUTER CONTROLLER OUTPUT:
          C1(k) = C1(k-1)+KC(1)*(b1c(1)*E1(k)-E1(k-1)-...
                TD(1)/tstep*(Y1m(k)-2*Y1m(k-1)+Y1m(k-2)));
      else
         if t(k) >= SP1ts
            C1(k) = SP_PRC(1);
         end
      end

%     windup on primary controller output
      
      % SET POINT OF INNER CONTROLLER:
      
      SP2(k) = C1(k)+Cff(k);     % recall that Cff=0 if in PRC mode

%     clamp on secondary set point after FF added
      if CASCADE == 1                % CHECK BOUNDS IF CASCADE
          SP2(k) = min (SP2(k), maxC(1)-Yinit(2)) ;
          SP2(k) = max (SP2(k), minC(1)-Yinit(2)) ;
          C1(k)  = SP2(k) - Cff(k);   % prevent primary reset windup
         
      end
%          
         
        % ADD DISTURBANCE TO OUTPUT X2:
         Y2(k) = X2(k) + N2(k);
         % ADD MEASUREMENT NOISE:
         Y2m(k) = Y2(k) + NY2(k);
         % INNER CONTROLLER INPUT:
         E2(k) = SP2(k) - Y2m(k)*CASCADE;
         % multiplying by flag CASCADE=0 will cancel inner controller
         % INNER CONTROLLER OUTPUT:
         if CASCADE == 1
             C2(k) = C2(k-1)+KC(2)*(b1c(2)*E2(k)-E2(k-1)-...
                   TD(2)/tstep*(Y2m(k)-2*Y2m(k-1)+Y2m(k-2)));
         else
            C2(k) = E2(k);
         end
%        windup check
         if CASCADE ==1
%            for cascade control, the primary controller could
%            windup when the final element reaches a limit
%            this calculation resets the primary output/sec SP
%            to the value that would bring the sec output just to its
%            limit - by back calculating from the PID result with C2(k)=limit
            if C2(k) <= minC(2)-MVinit
               %SP2(k) = Y2m(k); 
               holdd = (TD(2)/tstep)*(Y2m(k)-2*Y2m(k-1)+Y2m(k-2));
               E2(k)= (((minC(2)-MVinit)-C2(k-1))/KC(2) + E2(k-1)+holdd)/b1c(2);
               SP2(k) = E2(k) + Y2m(k);
               C1(k)  = SP2(k) ;  %- Cff(k);
            end
            if C2(k) >= maxC(2)-MVinit
               holdd = (TD(2)/tstep)*(Y2m(k)-2*Y2m(k-1)+Y2m(k-2));
               E2(k)= (((maxC(2)-MVinit)-C2(k-1))/KC(2) + E2(k-1)+holdd)/b1c(2);
               SP2(k) = E2(k) + Y2m(k);
               C1(k)  = SP2(k) ;  %- Cff(k);
            end   
         end
         C2(k) = min (C2(k), maxC(2)-MVinit) ;
         C2(k) = max (C2(k), minC(2)-MVinit) ; 
         

         if CASCADE ~= 1
             C1(k) = C2(k) - Cff(k);  %don't let SL windup
         end

        % INNER PROCESS, NOISE FREE PROCESS OUTPUT:
         X2(k+1) = - a1(2)*X2(k) - a2(2)*X2(k-1) + ...
                   b1(2)*C2(k-DD(2)) + b2(2)*C2(k-1-DD(2));  
      % OUTER PROCESS, NOISE-FREE PROCESS OUTPUT:
      X1(k+1) = -a1(1)*X1(k) - a2(1)*X1(k-1) + ...
                b1(1)*Y2(k-DD(1)) + b2(1)*Y2(k-1-DD(1));
   end % for
end % if

% CALCULATE PERFORMANCE INDICES
% -----------------------------
deltaC1 = C1(nsup+1:ntot) - C1(nsup:ntot-1);
deltaC2 = C2(nsup+1:ntot) - C2(nsup:ntot-1);
err1 = Y1m(ind) - SP1(ind);
err2 = Y2m(ind) - SP2(ind);
IAE1=sum(abs(err1))*tstep;     % calculates IAE for Y1m
IAE2=sum(abs(err2))*tstep;     % calculates IAE for Y2m
ISE1=sum(err1.^2)*tstep;       % calculates ISE for Y1m
ISE2=sum(err2.^2)*tstep;       % calculates ISE for Y2m
                        % MV performance, ---1 for SL ; ---2 for cascade
SAM1=(sum(abs(deltaC1))/tstep)*tstep;     % calculates sum of absolute deltaC1
SAM2=(sum(abs(deltaC2))/tstep)*tstep;     % calculates sum of absolute deltaC2=deltaMV
SSM1=(sum(deltaC2.^2)/(tstep^2))*tstep;   % calculates sum of squared deltaC1
SSM2=(sum(deltaC2.^2)/(tstep^2))*tstep;   % calculates sum of squared deltaC2=deltaMV

% ADD OFFSETS TO Y's AND C's
% --------------------------
% So that values have the right magnitudes on the plots
Y1m = Y1m + Yinit(1);
Y2m = Y2m + Yinit(2); 
SP1 = SP1 + Yinit(1);
SP2 = SP2 + Yinit(2);
if CASCADE == 1         % CASCADE STRUCTURE, ADD PROPER I.C.
    C1 = C1 + Yinit(2);
    C2 = C2 + MVinit;
else                    % Single loop structure, add proper I.C.
    C2 = C2 + MVinit;   %
end

CASCADEsave = CASCADE;  % Save current value for the graph routine;

%clf;
%close all
%text(0.32,0.50,'THE SIMULATION IS COMPLETE','sc');
disp (' ')
disp ('    THE SIMULATION IS COMPLETE '  )
pause(2);       % PAUSES FOR 1 SECOND
%close all