%
%  This function determines the poles for given state variable feedback gains for 
%  a second order system. It them simulates the system with these values
%
%  Amp = the step amplitude (in cm)
%   l = [k1 k2] where
%      k1 = position feedback gain
%      k2 = velocity feedback gain
%   kclg = closed loop gain
%   wn = estimated natural frequency
%   zeta = estimated dampling ratio
%   final_time = final time for the plot
%   filename = name of file you've edited that has the response of the
%      system. Put this in single quotes, like 'bob1.dat'
%      If you aren't comparing to data, just type '' for no file
%
function state_variables_1cart(Amp,k,kclg,wn,zeta,final_time,filename)
%
%  set up the system
%
  A = [0 1; -wn^2 -2*zeta*wn];
  B = [0; kclg*wn^2];
  C = [1 0];
  D = [0];
%
%  Now see where the poles are
%
 fprintf('The closed loop poles are: \n')
 disp(eig(A-B*k))
%
%  Find the system gain
%
  kpf = -1/(C*inv(A-B*k)*B);
  fprintf('K_pf = %f \n', kpf);
%
%  Next simulate the system
%
  H = ss(A-B*k,B*kpf,C,D);
  t = linspace(0,final_time,10000);
  u = Amp*ones(1,length(t));
  y = lsim(H,u,t);
%
%  Now plot the results
%
  if (~isempty(filename)) 
    encoder = 1;
    data = importdata(filename);
    tdata = data(:,encoder+1);
    ydata = data(:,encoder+3)/2196;
    plot(t,y,':',tdata,ydata,'-'); grid;
    ymin = min(min(y),min(ydata));
    ymax = max(max(y),max(ydata));
    axis([0 final_time ymin ymax]);
    legend('Model','Actual');
    xlabel('Time (sec)');
    ylabel('Displacement (cm)');
    title([' State Variable Feedback Response: k_1 = ',num2str(k(1),3),', k_2 = ',num2str(k(2),3), ', k_{pf} = ', num2str(kpf,4)]);
  else    % no data to compare to
    plot(t,y); grid;
    xlabel('Time (sec)');
    ylabel('Displacement (cm)');
    title([' State Variable Feedback Response: k_1 = ',num2str(k(1),3),', k_2 = ',num2str(k(2),3), ', k_{pf} = ', num2str(kpf,4)]); 
  end;
%
  return;