%
%  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
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  create the 1 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 bobs_1dof_model205  % this is the continuous time model from lab 1
  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
%

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

  Cy = [1 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(10*pi*time)*pi/180;
%  r = 10*time*pi/180;
%
%  set the weights
%
  Q = 0.8;  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_sv1_optimal_tracker');
%
%  plot the results
%
  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);
  end;
 %
  [st,sx1] = stairs(m_time,m_x1*scale);
  [st,sx1d] = stairs(m_time,m_x1_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);
  [mt,rr] = stairs(time,r*scale);
%
  figure
  orient landscape % plot sideways
% 
  subplot(2,2,1); 
  hold on
  plot(mt,rr,'r--','Linewidth',2);
  plot(st,sx1,'b-','Linewidth',2);
  grid; legend('Input','Output'); xlabel('Time (sec)'); ylabel('position_1 (deg)');
  title([' Q_f = ' num2str(Qf) ', Q = ' num2str(Q) ', R = ' num2str(R)]);
  hold off
%  
  subplot(2,2,3); plot(st,sx1d,'b-','Linewidth',2);
  grid; xlabel('Time (sec)'); ylabel('velocity_1 (deg)');
%  
  subplot(2,2,2); plot(st,soo,'b-','Linewidth',2);
  grid; xlabel('Time (sec)'); ylabel('control signal (deg)');
%  
  subplot(2,2,4);
  hold on
  plot(st,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 0.3 -saturation_level saturation_level]);
  hold off
 
  figure
  orient landscape % plot sideways
%  
  subplot(3,1,1); plot(st,skk1,'r-','Linewidth',2);
  grid; xlabel('Time (sec)'); ylabel('k_1');
%  
  subplot(3,1,2); plot(st,skk2,'r-','Linewidth',2);
  grid; xlabel('Time (sec)'); ylabel('k_2');
% 
  subplot(3,1,3); plot(st,skk3,'r-','Linewidth',2);
  grid; xlabel('Time (sec)'); ylabel('k_3');
  hold off