%  DYNAMIC SIMULATION

%  FILENAME: DYNSIM.M
%  DATE: August 24, 1993
%  VERSION: 01
%  COPYRIGHT 1993 M.Gretzinger & T.Marlin
%            McMaster University   

% This m-file is called from STABLE.M. It sets up a menu to enable the user
%       to specify the end time for integration and magnitude of the
%       time step and to execute the dynamic simulation of the system, 
%       using the parameters specified by the process and tuning data.

while 1
  clc
  disp('*************************************************************')
  disp('*                 DYNAMIC SIMULATION                        *')
  disp('*                                                           *')
  disp('*************************************************************')
  disp('')
  disp('SELECT THE APPROPRIATE MENU ITEM')
  disp('')
  disp('MODIFY...')
  disp('                                      PRESENT VALUES')
  fprintf('1) End time for integration           %4.2f\n',tend)
  fprintf('2) Time step                          %4.3f\n',deltat)
  if change == 's'
      disp('3) Select Set Point or Disturbance    s')
  else
      disp('3) Select Set Point or Disturbance    d')
  end

  disp('4) Execute dynamic simulation')
  disp('5) Return to main menu')
  disp('')
        
  m7 = input('Enter the desired selection:   ');
  
  if isempty(m7), break, end
      
  m7 = round(m7);
  if (m7<1) | (m7>5)
      disp('')
      disp('Not a valid selection')
      disp('')
      disp('Press ENTER to continue')
      pause;
  end    

  if m7 == 5
     
     break      %  return to main menu
  end

  if m7 == 1
      junktend = input('Enter value for end time of integration:  ') ;
      if isempty(junktend)
          junktend = tend ;
      end
      if junktend <= 0
          junktend = tend;
          disp('  ');
          disp('  TOTAL TIME MUST BE POSITIVE, PLEASE REENTER')
          pause
      end
      tend = junktend;
  
  elseif m7 == 2
      junkdeltat = input('Enter value for time step:  ') ;
      if isempty(junkdeltat)
          junkdeltat = deltat ;
      end
      if junkdeltat <= 0
          junkdeltat = deltat;
          disp('  ');
          disp('  TIME STEP MUST BE POSITIVE, PLEASE REENTER')
          pause
      end
      deltat = junkdeltat;


  elseif m7 == 3
      disp('For unit change in set point, enter the letter:    s')
      disp('For unit change in disturbance, enter the letter:  d')
      disp('')
      junkchange = input('Selection:  ','s') ;
      if junkchange ~= 'd' & junkchange ~= 's'
         junkchange = change;
         disp ('illegal entry, please reenter')
         disp ('press enter to continue')
         pause
      end
      if isempty(junkchange)
          junkchange = 'd' ;
      end
      change = junkchange;

                
  elseif m7 == 4      % execute dynamic simulation
      
      gamma = round(theta/deltat); 
      if (tend/deltat+gamma+2) > 7000
          deltat = (tend+theta)/(7000-2) ;
          gamma = round(theta/deltat);
          clc
          disp('Your choice of tend and deltat may lead to errors by')
          disp('surpassing matrix size and memory size limits. ')
          fprintf('To avoid this, deltat has been adjusted to %4.2f\n',deltat)
          disp('')
          disp('Press any key to continue')
          pause
      end

      clc
      if change == 'd'
        a=input('Are disturbance dynamics same as process dynamics?  [y/n]  ','s');
        if a ~= 'n' & a ~= 'y'
           a = 'y';
        end

        if a == 'y'
            thetad = theta; taulead_d = taulead; 
            taud1 = tau1; taud2 = tau2; taud3 = tau3; xid = xi;
        elseif a == 'n'
            thetad = 0; taulead_d = 0; 
            taud1 = taudist;  taud2 = 0; taud3 = 0; xid = 0;
        end
      elseif change == 's'
        thetad=0;  taulead_d=0;  taud1=0;  taud2=0; taud3=0; xid=0;
      end

      clc
      disp('Creating plot.....')
      disp('Press ENTER after viewing plot')

      % Initialize variables

      gamma = round(theta/deltat);        % round off to integer
      n = gamma + 2 + 1;
      t = 0;
      time = t  ;

      cv1 = zeros(1,tend/deltat+gamma+2) ;
      cv2 = cv1;  cv3 = cv2;  cv4 = cv3;  cv4b = cv4;  cv5 = cv4b;
      z = cv5;  e = z;  sp = e;  mv = e ;
      dv1 = cv1;  dv2 = cv2;  dv3 =cv3;  dv4 = cv4;  dv4b = cv4b;  dv5 = cv5;
      zd = z;  dist = sp;  cv = cv1;
      
      % If block to calculate values needed for second order 
      % process equations
      if tau3 ~= 0
          a2 = 2*xi/tau3 ;
          a1 = 1/tau3^2 ;
          a0 = a1 ;
          
          % When damping factor is >= 1, this block calculate tau4 and
          % tau5, which are then used as 2 lags in the process.
          if xi >= 1
             s = roots([tau3^2  2*xi*tau3  1]) ;
             tau4 = -1/s(1) ;
             tau5 = -1/s(2) ;
          end
      end

      % While loop which calculates the process, disturbance and controller
      % equations numerically, one element at a time, from time = 0 to 
      % time = tend, in increments of deltat.
      
      while t < tend

         % if block to determine value of set point or disturbance. 
         % At 5% of tend, a unit step in either the set point or the
         % disturbance occurs, as specified by the user.
         
         if t < .05*tend
             sp(n) = 0. ;  dist(n) = 0;
         elseif t >= .05*tend 
             if change == 'd'
                 dist(n) = 1. ;  sp(n) = 0. ;
             elseif change == 's'
                 dist(n) = 0. ;  sp(n) = 1. ;
             end
         end

         %PROCESS EQUATIONS
         %  GAIN
         cv1(n) = kp*mv(n-1) ;
         
         %  DEADTIME
         cv2(n) = cv1(n-gamma) ;

         %  LEAD/LAG
         if (tau1 == 0) & (taulead == 0)
            cv3(n) = cv2(n) ;
         else
            cv3(n) = (tau1*cv3(n-1)/deltat + (taulead/deltat+1)*cv2(n)...
               - taulead*cv2(n-1)/deltat) / (tau1/deltat+1) ;
         end

         %  LAG
         if tau2 == 0
            cv4(n) = cv3(n);
         else
            cv4(n) = (1-exp(-deltat/tau2))*cv3(n)... 
                      + (exp(-deltat/tau2))*cv4(n-1) ;
         end
      
         %  SECOND ORDER
         if tau3 == 0
            cv5(n) = cv4(n);
         else
            if xi < 1
               cv5(n) = cv5(n-1) + deltat*z(n-1) ;
               z(n) = z(n-1) + deltat*(-a2*z(n-1) - a1*cv5(n-1) + a0*cv4(n-1));

            elseif xi >= 1    % use tau4 and tau5 calculated above (line 77-83)
                              % and solve as 2 more lags

               cv4b(n) = (1-exp(-deltat/tau4))*cv4(n)... 
                         + (exp(-deltat/tau4))*cv4b(n-1) ;
               cv5(n) = (1-exp(-deltat/tau5))*cv4b(n) ...
                        + (exp(-deltat/tau5))*cv5(n-1) ;
            end
         end


         %  DISTURBANCE EQUATIONS


         %  GAIN
         dv1(n) = kpd*dist(n) ;
         
         %  DEADTIME
         dv2(n) = dv1(n-gamma) ;

         %  LEAD/LAG
         if (taud1 == 0) & (taulead_d == 0)
            dv3(n) = dv2(n) ;
         else
            dv3(n) = (taud1*dv3(n-1)/deltat + (taulead_d/deltat+1)*dv2(n)...
               - taulead_d*dv2(n-1)/deltat) / (taud1/deltat+1) ;
         end

         %  LAG
         if taud2 == 0
            dv4(n) = dv3(n);
         else
            dv4(n) = (1-exp(-deltat/taud2))*dv3(n)... 
                      + (exp(-deltat/taud2))*dv4(n-1) ;
         end
      
         %  SECOND ORDER
         if taud3 == 0
            dv5(n) = dv4(n);
         else
            if xid < 1
               dv5(n) = dv5(n-1) + deltat*zd(n-1) ;
               zd(n) = zd(n-1) + deltat*(-a2*zd(n-1) - a1*dv5(n-1) + a0*dv4(n-1));

            elseif xid >= 1    % use tau4 and tau5 calculated above (line 77-83)
                              % and solve as 2 more lags

               dv4b(n) = (1-exp(-deltat/tau4))*dv4(n)... 
                         + (exp(-deltat/tau4))*dv4b(n-1) ;
               dv5(n) = (1-exp(-deltat/tau5))*dv4b(n) ...
                        + (exp(-deltat/tau5))*dv5(n-1) ;
            end
         end

         cv(n) = cv5(n) + dv5(n) ;   % Add output from process and disturbance
                                     % to send to controller

         %  CONTROLLER EQUATIONS
         
         if kc == 0
            mv(n) = mv(n-1) ;
         else
            e(n) = sp(n) - cv(n) ;
            % If block to ensure no division by zero errors when no integral
            % mode is present in controller
            if ti == 0
                deltamv = kc*(e(n) - e(n-1) - td*(cv(n) - 2*cv(n-1) ...
                              + cv(n-2))/deltat) ;
            else   
                deltamv = kc*(e(n) - e(n-1) + deltat*e(n)/ti ...
                              - td*(cv(n) - 2*cv(n-1) + cv(n-2))/deltat) ;
            end
            mv(n) = mv(n-1) + deltamv ;
         end

        % Update t and n. Store next t value in time, the vector of times
        % at which each calculation is carried out.
         t = t + deltat ;
         time = [time t] ;
         n = n+1 ;

         
      end       % while t < tend

  % Calculate IAE (Integral of the Absolute value of the Error)
  IAE = 0;
  IAE = deltat * sum(abs(e)) ;
 
  % Plot set point (dashed green line) and controlled variable (solid red
  % line) versus time.
  clf ; axis('auto')
  subplot(211),plot(time,sp(1,round(theta/deltat+2):length(sp)),'--g', time,cv(1,round(theta/deltat+2):length(cv)))
  title(['DYNAMIC SIMULATION   (IAE = ',num2str(IAE),')'])
  xlabel('Time')
  ylabel('Controlled Variable')

  % Plot manipulated variable versus time
  subplot(212),plot(time,mv(1,round(theta/deltat+2):length(mv)))
  xlabel('Time')
  ylabel('Manipulated Variable')
  
  figure(1)
  pause
  close all

  clear cv1 cv2  cv3 cv4 cv4b cv5 z e sp mv dv1 dv2 dv3 dv4 dv4b dv5 
  clear zd dist cv time
  
  else
      break
  end        % if m7
  
end    % while 1 (line 12)