% Workshop Solution (r_hw2)
% MPC Workshop, Trondheim (January 2015): digial PID Workshop
%   
%   Define continuous time model (3 tank)
%   Discretize using c2d (with 0.5 min sample time)
%   Discrete PID (function: dpid.m)
%     apply to simulated linear plant (linodepar.m)
%   Find discrete closed-loop poles
%
% MPC 2013, B.W. Bequette, 12 Sept 13
%  function files used: dpid.m and lindoepar.m
%
% first, continuous state space model
  a = [-1 0 0;1 -1 0;0 1 -1]
  b = [1;0;0]
  c = [0 0 1]
  d = 0
%
  lintank = ss(a,b,c,d)              % defines continuous state space model
  [yc,tc] = step(lintank);           % generate step response
% --------------------------------------------------------------------
%   Problem 1
% discrete time model
  delt = 0.5;                        % sample time of 0.5 minutes
  tankssz = c2d(lintank,delt,'zoh')  % create discrete state space model from continuous
  [yd,td] = step(tankssz);           % generate step response of discrete model
  figure(11)
  plot(tc,yc,'k',td,yd,'k--')        % compare step responses of continous
%                                      and discrete models
  xlabel('t, min')
  ylabel('y')
  legend('continuous','discrete')
  title('continuous vs. discrete, three tank step response')
% discrete process transfer function
  tanktfz = tf(tankssz)              % create discrete t.f. from discrete state space model
  [nump,denp,tsample] = tfdata(tanktfz,'v')  % get the numerator and denominator polynomials
  roots(nump)                        % finds zeros of discrete transfer function
  roots(denp)                        % finds poles of discrete transfer function
  tanktzpk = zpk(tankssz)            % zero-pole-gain form (consistency check)
%
% --------------------------------------------------------------------
%   Problem 2
% setup closed-loop simulation
  x0 = zeros(3,1);                   % state vector initial condition
  y0 = c*x0;                         % initial output
  u0 =   0;                          % initial input
  tbeg = 0;                          % simulation start time
  tend =20;                          % simulation end time
  kc = 1.25;                         % controller proportional gain
  taui = 3;                          % controller integral time
  taud = 0.4;                        % controller derivative time
  par = [kc;taui;taud];              % tuning parameter vector
%
% now, set up a for loop and integrate over 0.5 minute time steps
% currently set up for single input - single output
%
  delt = 0.5;                        % sample time = 0.5 minutes
  time = tbeg:delt:tend;             % generate the time vector
  rsp = [zeros(2,1);ones(length(time)-2,1)]; % setpoint change at third time step
  xdis(:,1) = x0;                    % initial state vector for discrete time simulation
  ydis(1)   = y0;                    % initial output for discrete time simulation
  for k = 1:length(time)-1;          % k = index for simulation
%
    if k == 1;
        rvec = [0;0;rsp(1)];
        yvec = [0;0;y0];
        u(k) = dpid(delt,par,rvec,yvec,u0);
    elseif k == 2;
        rvec = [0;rsp(1);rsp(2)];
        yvec = [0;y0;ydis(1)];
         u(k) = dpid(delt,par,rvec,yvec,u(k-1));
    else
         rvec = rsp(k-2:k);
         yvec = ydis(k-2:k);
         u(k) = dpid(delt,par,rvec,yvec,u(k-1));
    end
 %
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      ndum = length(tdummy);          
      xdis(:,k+1) = xdummy(ndum,:)';
      ydis(k+1) = c*xdis(:,k+1);
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
  figure(12)
  subplot(2,1,1)
  plot(time,ydis,time,rsp,'--')
  title('setpoint response, PID control')
  xlabel('t, min')
  ylabel('y, m')
  legend('y','r')
  subplot(2,1,2)
  plot(time,u)
  xlabel('t, min')
  ylabel('u')
%
% prefer zero order hold on inputs
%
  [tt,uu] = stairs(time,u);           % zero-order hold on input 
  [ttrsp,rrsp] = stairs(time,rsp);    % zero-order hold on setpoint
%
  figure(13)
  subplot(2,1,1)
  plot(time,ydis,ttrsp,rrsp,'--')
  legend('y','r')
  ylabel('y, m')
  title('kc = 1.25')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
% -----------------------------------------------------------------------------
%  Problem 3 - simulation results
%              vary kc until closed-loop is on the verge of instability
  kc = 4.885;
%
  par = [kc;taui;taud];      % tuning parameter vector (passed into dpid function)
%
% now, set up a for loop and integrate over 0.5 minute time steps
% currently set up for single input - single output
%
  delt = 0.5;  % sample time = 0.5 minutes
  time = tbeg:delt:tend;                     % generate the time vector
  rsp = [zeros(2,1);ones(length(time)-2,1)]; % setpoint change at third step
  xdis(:,1) = x0;                            % discrete state initial condition
  ydis(1)   = y0;                            % output initial condition
  for k = 1:length(time)-1;
    if k == 1;
        rvec = [0;0;rsp(1)];
        yvec = [0;0;y0];
        u(k) = dpid(delt,par,rvec,yvec,u0);
    elseif k == 2;
        rvec = [0;rsp(1);rsp(2)];
        yvec = [0;y0;ydis(1)];
         u(k) = dpid(delt,par,rvec,yvec,u(k-1));
    else
         rvec = rsp(k-2:k);
         yvec = ydis(k-2:k);
         u(k) = dpid(delt,par,rvec,yvec,u(k-1));
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:)';
      ydis(k+1) = c*xdis(:,k+1);
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
% prefer zero order hold on inputs
%
  [tt,uu] = stairs(time,u);           % zero-order hold on input
  [ttrsp,rrsp] = stairs(time,rsp);    % zero-order hold on setpoint
%
  figure(14)
  subplot(2,1,1)
  plot(time,ydis,ttrsp,rrsp,'--')
  legend('y','r')
  ylabel('y, m')
  title('on the verge of instability, kc = 4.885')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
% -------------------------------------------------------------------------
%   Problem 3 - find the closed-loop poles (poles of 1 + gc*gp)
%               
%  first, coefficients of controller numerator (different order than some
%  notes)
%         uses kc from previous simulation
  bc2 = kc*(1+(delt/taui)+(taud/delt));
  bc1 = -kc*(1+2*(taud/delt));
  bc0 = kc*taud/delt;
% 
  numc = [bc2 bc1 bc0];           % numerator of controller transfer function
  denc = [1   -1    0];           % denominator of controller transfer function
%                                   process numerator and denominator were
%                                   found in problem 1
  denpdenc = conv(denp,denc)      % multiply process and controller denominators
  numpnumc = conv(nump,numc)      % multiply process and controller numerators
  denpdenc + numpnumc             % add terms
  clpoles = roots(denpdenc + numpnumc)       % find closed-loop poles
  abs(clpoles)                    % calculates the magnitude of each pole 
%                                   (important for complex conjugate poles)
%  create a unit circle
  angle = 0:0.1:2*pi;
  xcirc = cos(angle);
  ycirc = sin(angle);
%  plot the closed-loop poles, superimposed on the unit circle
  figure(16)
  plot(real(clpoles),imag(clpoles),'+',xcirc,ycirc)
  axis square                       % make a square plot
  title('on the verge of instability, kc = 4.885')
% ------------------------------------------------------------------------
%     Problem 4 - simulation for unstable tuning
%               
    kc = kc*1.1;               % increase previous kc by 10%
    par = [kc;taui;taud];      % tuning parameter vector
%
% now, set up a for loop and integrate over 0.5 minute time steps
% currently set up for single input - single output
%
  delt = 0.5;  % sample time = 0.5 minutes
  time = tbeg:delt:tend;       % generate the time vector
  rsp = [zeros(2,1);ones(length(time)-2,1)]; % setpoint change at third step
  xdis(:,1) = x0;
  ydis(1)   = y0;
  for k = 1:length(time)-1;
    if k == 1;
        rvec = [0;0;rsp(1)];
        yvec = [0;0;y0];
        u(k) = dpid(delt,par,rvec,yvec,u0);
    elseif k == 2;
        rvec = [0;rsp(1);rsp(2)];
        yvec = [0;y0;ydis(1)];
         u(k) = dpid(delt,par,rvec,yvec,u(k-1));
    else
         rvec = rsp(k-2:k);
         yvec = ydis(k-2:k);
         u(k) = dpid(delt,par,rvec,yvec,u(k-1));
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:)';
      ydis(k+1) = c*xdis(:,k+1);
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
% prefer zero order hold on inputs
%
  [tt,uu] = stairs(time,u);
  [ttrsp,rrsp] = stairs(time,rsp);
  figure(18)
  subplot(2,1,1)
  plot(time,ydis,ttrsp,rrsp,'--')
  legend('y','sp')
  ylabel('y')
  title('unstable closed-loop response, kc = 1.1*4.885')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
% -------------------------------------------------------------------------
% 
%   Problem 4 - find the closed-loop poles (poles of 1 + gc*gp)
%               
%  first, coefficients of controller numerator
%
  bc2 = kc*(1+(delt/taui)+(taud/delt));
  bc1 = -kc*(1+2*(taud/delt));
  bc0 = kc*taud/delt;
% 
  numc = [bc2 bc1 bc0];
  denc = [1   -1    0];
%
  denpdenc = conv(denp,denc)
  numpnumc = conv(nump,numc)
  denpdenc + numpnumc
  clpoles = roots(denpdenc + numpnumc)       % find closed-loop poles
  abs(clpoles)
%     create unit circle
  angle = 0:0.1:2*pi;
  xcirc = cos(angle);
  ycirc = sin(angle);
%
  figure(19)
  plot(real(clpoles),imag(clpoles),'+',xcirc,ycirc)
  axis square
  title('unstable poles, kc = 1.1*4.885')
% ----------------------------------------------------------------------
%
%  Again, simulating problem 4 (unstable), using more points to make the
%     plots smoother
  tplot = [];  % new vectors for plotting that will yield smoother plots
  yplot = [];  
%
  delt = 0.5;  % sample time = 0.5 minutes
  time = tbeg:delt:tend;       % generate the time vector
  rsp = [zeros(2,1);ones(length(time)-2,1)]; % setpoint change at third step
  xdis(:,1) = x0;
  ydis(1)   = y0;
  for k = 1:length(time)-1;
    if k == 1;
        rvec = [0;0;rsp(1)];
        yvec = [0;0;y0];
        u(k) = dpid(delt,par,rvec,yvec,u0);
    elseif k == 2;
        rvec = [0;rsp(1);rsp(2)];
        yvec = [0;y0;ydis(1)];
         u(k) = dpid(delt,par,rvec,yvec,u(k-1));
    else
         rvec = rsp(k-2:k);
         yvec = ydis(k-2:k);
         u(k) = dpid(delt,par,rvec,yvec,u(k-1));
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:)';
      ydis(k+1) = c*xdis(:,k+1);
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
% prefer zero order hold on inputs
%
  [tt,uu] = stairs(time,u);
  [ttrsp,rrsp] = stairs(time,rsp);
  figure(20)
  subplot(2,1,1)
  plot(tplot,yplot,ttrsp,rrsp,'--')
  legend('y','sp')
  ylabel('y')
  title('unstable closed-loop response, kc = 1.1*4.885, more points for smoother plots')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')