%
%  Matlab driver for the basic 1-dof state variable model
%
%  set up the matrices for constructing the states
%  this is for the ECP Simulink program
%
  get_positions = [1 0 0 0 0; 
                   0 1 0 0 0; 
                   0 0 1 0 0];
  get_angles = [0 0 0 1 0];
  get_error_bit = [0 0 0 0 1];
  get_all_states = [1 0 0 0 0 0 0 0; 
                    0 0 0 1 0 0 0 0; 
                    0 1 0 0 0 0 0 0; 
                    0 0 0 0 1 0 0 0; 
                    0 0 1 0 0 0 0 0; 
                    0 0 0 0 0 1 0 0; 
                    0 0 0 0 0 0 1 0; 
                    0 0 0 0 0 0 0 1];
 %
 % this is the only part you should ever have to change
 %
 get_desired_states = [1 0 0 0 0 0 0 0; 
                       0 1 0 0 0 0 0 0];
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  set the saturation levels
%
  saturation_level = 1000/2196;   % model 210
%  saturation_level = 1000/2546;   % model 205
%
%  set the input amplitude (in cm or radians)
%
  Amp = 1;
%  Amp = 15*pi/180;
%
%  set the final time
%
  Tf = 2.0;
%
%  load the A, B, C, and D matrices
%
  load state_model_1dof_210
% load state_model_1dof_205
  C = [1 0];
%
%  Now find the state feedback vector
%
%  [n,m] = size(A);
%  K = lqr(A,B,diag([10 0.05]),100)
  
  p = [-3 -4];
  K = place(A,B,p);
  disp('Feedback gains are:' ); K
  disp('Closed loop poles at :'); eig(A-B*K)
%
%  set the initial conditions
%
  ic_x = 0;
  ic_x_dot = 0;
%
%  determine the prefilter gain
%
  Gpf = -1/(C*inv(A-B*K)*B)
%
%  run the simulation
%
  sim('Basic_1dof_State_Variable_Model');
%
%  Now plot the data
%
  ubound = saturation_level*ones(1,length(m_time));
  orient landscape
  subplot(2,2,1); plot(m_time,m_x1); grid; title('Cart Position (cm)');
  subplot(2,2,3); plot(m_time,m_x1_dot); grid; title('Cart Velocity (cm/sec)');
  subplot(2,2,2); plot(m_time,m_y,'-',m_time,m_r,':'); grid; legend('Output','Input',4); title('Input/Output');
  subplot(2,2,4); plot(m_time,m_u,'-',m_time,ubound,':',m_time,-ubound,':'); grid; title('Control Effort');
  axis([0 0.2 -saturation_level saturation_level]);
  disp([' k_1 = ' num2str(K(1)) ', k_2 = ' num2str(K(2)) ', G_{pf} = ' num2str(Gpf) ', Closed Loop poles at ' num2str(eig(A-B*K)')]);