%
%  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 = 0.5;
  Amp = 10*pi/180;
%
%  set the final time
%
  Tf = 2.0;
%
%  load the A, B, C, and D matrices
%
%  load state_model_1dof_rect
  load state_model_1dof_tor
  C = [1 0];
%
%  Now find the state feedback vector
%
%  [n,m] = size(A);
%  K = lqr(A,B,diag([10 0.05]),100)
  
  p = [-8 -9];
  K = place(A,B,p);
  disp('Feedback gains are:' ); K
  disp('Closed loop poles at :'); eig(A-B*K)
%
%  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
%
  scale = 1.0;
  scale = 180/pi;
  ubound = saturation_level*ones(1,length(m_time));
  orient landscape
  subplot(2,2,1); plot(m_time,m_x1*scale); grid; title('Cart Position (deg)');
  subplot(2,2,3); plot(m_time,m_x1_dot*scale); grid; title('Cart Velocity (deg/sec)');
  subplot(2,2,2); plot(m_time,m_y*scale,'-',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)')]);