%
%  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; 
%
%  set the input amplitude (in cm)
%
  Amp = 1;
%
%  set the final time
%
  Tf = 3.0;
%
%  load the A, B, C, and D matrices
%
  load state_model
%
%  Now find the state feedback vector
%
  [n,m] = size(A);
  %K = lqr(A,B,diag([10 0.03]),100)
  K = place(A,B,[-6 -7])
  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('Input','Output',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]);