%
%  closed loop driver-- examine the response of the plant by itself
%
%  put in the saturation limits for the ECP 210  or 205 systems
%
  saturation_level = 1000/2546; % ECP 205
  saturation_level = 1000/2196; % ECP 210 
%  
%  now load the plant model
%
  load state_model_1dof_205
% load state_model_1dof_model210
%
%  we want the output to be the two states
%
  C = eye(2,2);
%
%  convert to a Matlab state space (ss) model
%
  Hc = ss(A,B,C,D);
%
%  set the final time for the simulation
%
  Tf = 2.0;  % seconds
%  
%  set the amplitude and final time
%
%  Amp = 1 * (pi/180); %the input, in radians, for model 205 
  Amp = 0.1; % the input, in cm for model 210
%
%  set the input frequency for a sine wave
%
   freq = 1*2*pi;  % be sure this in in radians/sec
%
%  first we will simulate the continuous time system
%
  t = linspace(0,Tf,100000);
  u = Amp*ones(1,length(t));
 % u = Amp*sin(freq*t);
  y = lsim(Hc,u,t);
%
%  now simulate the discrete-time system
%
%  set the sample interval
%
   Ts = 0.05;
%  
%  discretize the state space model using a zero order hold, then get
%  the state variable model back
%
  Hd = c2d(Hc,Ts,'zoh');
  [G,H,C,D] = ssdata(Hd);
%
%  now simulate the system
%
  sim('DT_Openloop');
%
%  plot the results
%
%  we want to plot the discrete-time system using the stairs commmand
%  (format)
%
  [td,x1_d] = stairs(m_time,m_x1);
  [td,x2_d] = stairs(m_time,m_x1_dot);
%
%  Now we are ready to plot
%
  figure
  orient landscape % plot sideways
  subplot(2,1,1); 
    hold on
    plot(t,y(:,1),'b-','Linewidth',2);
    plot(td,x1_d,'r-','Linewidth',1);
    grid; xlabel('Time (sec)'); ylabel('Position (cm)');
    hold off
  subplot(2,1,2); 
    hold on
    plot(t,y(:,2),'b-','Linewidth',2);
    plot(td,x2_d,'r-','Linewidth',1);
    grid; xlabel('Time (sec)'); ylabel('Velocity (cm/sec)');
    hold off