%
%  This routine runs the Simulink model for the
%    optimal tracker wtih soft terminal constraints
%  
  clear
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
  saturation_level = 1000/2196; % set the limiter for the model 210
  saturation_level = 1000/2546; % set the limiter for the model 205
%  saturation_level = 100;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  create the 2 dof discrete-time model with the desired sampling times
%
%  set the sampling time
%
  Ts = 0.03;
%
%  first read in the continuous time model, sample it, and put it into 
%  a form that represents the 1-sample delay
%
  load  ct_state_model_2dof_model205 %
  
  Hc = ss(A,B,C,D);
%  
%  discretize the state space model, then get
%  the state variable model back
%
  Hd = c2d(Hc,Ts,'zoh');
  [GG,HH,CC,DD] = ssdata(Hd);
%
%  Now construct the discrete-time state model with the delay
%
%  for the 1 dof

  G = [GG HH; zeros(1,5)];
  H = [zeros(4,1); 1];
  C = eye(5,5);
  D = 0;
%
%  all done
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  Cy = [1 0 0 0 0];
%  
%  set the final time
 
  Tf = 1.0;  % seconds
%
  time = [0:Ts:Tf];
%
%  determine the input we want to track
%
  r = 10*ones(1,length(time))*pi/180;
%   r = 15*sin(7*pi*time)*pi/180;
%   r = 10*time*pi/180;
%
%  set the weights
%
  Q = 0.01;  R = 0.1; Qf = 0.05;
%  
%  get the feedforward control and feedback gains
%
  [K, uf, Kss, uf_ss] = optimal_tracker(G,H,Cy,Q,R,Qf,r);
% 
  disp('Closed loop poles are nominally at ')
  eig(G-H*Kss)
%
%  set up the data to be read by Simulink
%
  uff = [];
  u_ss = [];
  values = [];
  values_ss = [];
  times = [];
  u = [];
  for k = 1:length(uf)
    u = [u; uf(k).a];
    values = [values; K(k).a];
    times = [times; (k-1)*Ts];
    u_ss = [u_ss; uf_ss(k).a];
    values_ss = [values_ss; Kss];
  end;
    uff = [times u];
    KK.time = times;
    KK.signals.values = values ;
 %
 %  uncomment the following two lines for constant K
 %
%      uff = [times u_ss];
%      KK.signals.values = values_ss;
 %
    KK.signals.dimensions = length(K(1).a);
%
%  now simulate the system
%
  sim('DT_sv2_optimal_tracker');
%
%  scale the data
%  
  scale = 180/pi; 
%  scale = 1.0;
%
%  Convert the Simulink data
%
 for i=1:length(m_k)
    m_k1(i) = m_k(i,1);
    m_k2(i) = m_k(i,2);
    m_k3(i) = m_k(i,3);
    m_k4(i) = m_k(i,4);
    m_k5(i) = m_k(i,5);
  end;
 %
  [st,sx1] = stairs(m_time,m_x1*scale);
  [st,sx1d] = stairs(m_time,m_x1_dot*scale);
  [st,sx2] = stairs(m_time,m_x2*scale);
  [st,sx2d] = stairs(m_time,m_x2_dot*scale);
  [st,soo] = stairs(m_time,m_uf*scale);
  [st,suu] = stairs(m_time,m_u);
  [st,skk1] = stairs(m_time,m_k1);
  [st,skk2] = stairs(m_time,m_k2);
  [st,skk3] = stairs(m_time,m_k3); 
  [st,skk4] = stairs(m_time,m_k4);
  [st,skk5] = stairs(m_time,m_k5);
  [mt,rr] = stairs(time,r*scale);
%
  figure
  orient landscape % plot sideways
%
  subplot(3,2,1); 
  hold on
  if (Cy == [1 0 0 0 0])
    plot(mt,rr,'r--','Linewidth',2);
    plot(st,sx1,'b-','Linewidth',2);
    legend('Input','Output');
    title(['Q_f = ' num2str(Qf) ', Q = ' num2str(Q) ', R = ' num2str(R)]);
  else
    plot(st,sx1,'b-','Linewidth',2);
  end;
  grid; xlabel('Time (sec)'); ylabel('position_1 (deg)');
  hold off
%  
  subplot(3,2,3); plot(st,sx1d,'b-','Linewidth',2);
  grid; xlabel('Time (sec)'); ylabel('velocity_1 (deg)');
% 
  subplot(3,2,2); 
  hold on
  if (Cy == [0 0 1 0 0])
    plot(mt,rr,'r--','Linewidth',2);
    plot(st,sx2,'b-','Linewidth',2);
    legend('Input','Output');
    title(['Q_f = ' num2str(Qf) ', Q = ' num2str(Q) ', R = ' num2str(R)]);
 else
    plot(st,sx2,'b-','Linewidth',2);
 end;
  grid; xlabel('Time (sec)'); ylabel('position_2 (deg)');
  hold off
%  
  subplot(3,2,4); plot(st,sx2d,'b-','Linewidth',2);
  grid; xlabel('Time (sec)'); ylabel('velocity_2 (deg)');
%  
  subplot(3,2,5); plot(st,soo,'b-','Linewidth',2);
  grid; xlabel('Time (sec)'); ylabel('control signal (deg)');
%  
  subplot(3,2,6); 
  hold on
  plot(mt,suu,'b-','Linewidth',2);
  grid; xlabel('Time (sec)'); ylabel('control effort');
  plot(time,saturation_level*ones(1,length(time)),'m','Linewidth',3);
  plot(time,-saturation_level*ones(1,length(time)),'m','Linewidth',3);
  axis([0 Tf -saturation_level saturation_level]);
  hold off
%  
%   figure
%   orient landscape % plot sideways
% %
%   subplot(3,2,1); plot(st,skk1,'r-','Linewidth',2);
%   grid; xlabel('Time (sec)'); ylabel('k_1');
% %  
%   subplot(3,2,2); plot(st,skk2,'r-','Linewidth',2);
%   grid; xlabel('Time (sec)'); ylabel('k_2');
% %  
%   subplot(3,2,3); plot(st,skk3,'r-','Linewidth',2);
%   grid; xlabel('Time (sec)'); ylabel('k_3');
% %  
%   subplot(3,2,4);  plot(st,skk4,'r-','Linewidth',2);
%   grid; xlabel('Time (sec)'); ylabel('k_4');
% %  
%   subplot(3,2,5); plot(st,skk5,'r-','Linewidth',2);
%   grid; xlabel('Time (sec)'); ylabel('k_5');
% %
%   subplot(3,2,6);  plot(st,soo,'r-','Linewidth',2);
%   grid; xlabel('Time (sec)'); ylabel('control signal (deg)');
%  
%  
