%
%  closed loop driver-- examine the response of the plant by itself
%
%  this sets the value of the limiter
%
  saturation_level = 1000/2196; %  for the model 210
% saturation_level = 1000/2546; %  for the model 205 (torsional)
%
%  now load the plant model
%  the model is in state space form and we need to
%    convert it to transfer function form
%    don't worry about this for now
%
  load state_model_1dof_rect           % load your model file here
  C = [1 0];
  [num_Gp,den_Gp] = ss2tf(A,B,C,D);
%
%  clean up the transfer function by setting small values to 0
%
  tol = 1e-8;    % anything smaller than this is set to zero
%
% you are not responsible for the following voo doo
%
  num_Gp = (num_Gp > tol*ones(1,length(num_Gp))).*num_Gp;
  den_Gp = (den_Gp > tol*ones(1,length(den_Gp))).*den_Gp;
%
%  now construct the transfer function and display it
%
  Gp = tf(num_Gp,den_Gp);
  disp('Gp is : '); Gp  
%
%=========================================================
%  This is model matching stuff
%
%  get the closed loop transfer function
%
%  first the zero position error ITAE
%
   wo = 25;
  Go = tf(wo^2,[1 1.4*wo wo^2]);
%  Go = tf(wo^3,[1 1.75*wo 2.15*wo^2 wo^3]);
%  Go = tf(wo^4,[1 2.1*wo 3.4*wo^2 2.7*wo^3 wo^4]);
%
%  next the zero velocity error ITAE
%
%  Go = tf([3.2*wo wo^2],[1 3.2*wo wo^2]);
%  Go = tf([3.25*wo^2 wo^3],[1 1.75*wo 3.25*wo^2 wo^3]);
%  Go = tf([5.14*wo^3 wo^4],[1 2.41*wo 4.93*wo^2 5.14*wo^3 wo^4]);
%
%  Finally the deadbeat systems
%
%  Go = tf(wo^2, [1 1.82*wo wo^2]);
%  Go = tf(wo^3, [1 1.90*wo 2.20*wo^2 wo^3]);
%  Go = tf(wo^4, [1 2.20*wo 3.50*wo^2 2.80*wo^3 wo^4]);
%
%   determine the controller
%
  Gc = Go/(Gp*(1-Go));  
%
%============================================================
%
%  Now get rid of pole/zero cancellations in the controller
%  and put it in the correct form for Simulink
%
  disp('Gc is :');
  Gc = minreal(Gc,tol)
  [num_Gc,den_Gc] = tfdata(Gc,'v') ;
%
%=============================================================
%
%  determine the prefilter, for now it is just 1
%
  num_Gpre = 1;
  den_Gpre = 1;
%
%==============================================================
%
%  set the step amplitude and final time
%
  Amp = 1.0; % cm for the ECP 210 system
  Tf = 1.0;  % seconds
%
%  now simulate the system
%
  sim('closedloop');
%
%  plot the results
%
  subplot(2,1,1); plot(m_time,m_y,'b-','Linewidth',2); grid; xlabel('Time (sec)'); ylabel('Cart 1 (cm)');
  title(['\omega_0 = ' num2str(wo) ' rad/sec ']);
  subplot(2,1,2); hold on
  plot(m_time,m_u,'r-','Linewidth',2); 
  plot(m_time,-saturation_level*ones(1,length(m_time)),'--','Linewidth',2);
  plot(m_time,saturation_level*ones(1,length(m_time)),'--','Linewidth',2);
  hold off
  grid; xlabel('Time (sec)'); ylabel('Control Effort');
 % axis([0 0.1 -0.5 0.5]);
   