%
%  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;
%
%  first read in the continuous time model, sample it, and put it into 
%  a form that represents the 1-sample delay
%
  load state_model_1dof_rect
  Hc = ss(A,B,C,D);
%  
%  discretize the state space model, then get
%  the state variable model back
%
  Hd = c2d(Hc,Ts,'zoh');
  [GG,HH,CC,DD] = ssdata(Hd);
%
%  Now construct the discrete-time state model with the delay
%
  Gc = [GG HH; zeros(1,3)];
  Hc = [zeros(2,1); 1];
  Cc = eye(3,3);
  Dc = 0;
%
  G = Gc; H = Hc; C = Cc; D = Dc;
  save c2d_1dof_model210 G H C D 
%
%  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.4 0.5 0.6];
  K = place(G,H,p);
%  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 1.0 -saturation_level saturation_level]);
