%
%  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];
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
  saturation_level = 1000/2196; % set the limiter for the model 210
%  saturation_level = 1000/2546; % set the limiter for the model 205
%
%  set the sampling time
%
  Ts = 0.05;
%
%  now load the plant model, in discrete variable form
%
  load c2d_1dof_model210
  C = [1 0 0];
%
% find the state variable feedback gains
%
  p = [0.2 0.2 0.2];
  K = acker(G,H,p)
  eig(G-H*K)
%
%  set the prefilter gain
%
  GG = G-H*K
  eye(size(GG))-GG
  Gpf = inv(C*inv(eye(size(GG))-GG)*H)
%  
%  set the step amplitude and final time
%
  Amp = 1.0; % desired final position, in cm
 % Amp = 15*pi/180; % desired final position, in rad
  Tf = 1.0;  % seconds
%
%  now simulate the system
%
  sim('DT_sv1');
%
%  plot the results
%
  figure
  orient landscape % plot sideways
  scale = 180/pi; 
  scale = 1.0;
  subplot(2,2,1); stairs(m_time,m_x1*scale); grid; xlabel('Time (sec)'); ylabel('Position (cm)');
  subplot(2,2,3); stairs(m_time,m_x1_dot*scale); grid; xlabel('Time (sec)'); ylabel('Velocity (cm/sec)');
  subplot(2,2,2); stairs(m_time,m_y*scale); grid; xlabel('Time (sec)'); ylabel('Output');
  subplot(2,2,4); stairs(m_time,m_u); grid; xlabel('Time (sec)'); ylabel('Control Effort');
  % axis([0 0.1 -saturation_level saturation_level]);
