% Beam Main Routine
% for ECE 463 Final (sp 18)


X = [0, 0, 0, 0]';
dt = 0.001;
t = 0;

% A = [0,0,1,0;0,0,0,1;0,-7,0,0;-8.91,0,0,0];
% B = [0;0;0;1.8182];

Kx = [  -20.5174   21.2219   -8.9938    4.8316];
Kz = -10.0000;
Z = 0;

% Observer
A = [0,0,1,0;0,0,0,1;0,-7,0,0;-8.91,0,0,0];
B = [0;0;0;1.8182];
C = [1,0,0,0;0,1,0,0];

Xo = X;

y = [];

while(t < 15)
   for i=1:10
      Ref = sin(t);
 
% for one cycle, use the actual states (cheating)
% to let the observer states converge. Then switch
% to using the observer

      if (t < 6.28) U = -Kz*Z - Kx*X;
      else          U = -Kz*Z - Kz*Xo;
      end

      Y = C*X + [0.01*randn ; 0.02*randn];
      Yo = C*Xo;
 
      dX = BeamDynamics(X, U);
      dZ = (Yo(1) - Ref);
 
      X = X + dX * dt;
      Z = Z + dZ * dt;
      Xo = X;   % faked observer 
      t = t + dt;
      end

    y = [y ; Ref, X(1), Xo(1)];
 
    BeamDisplay(X, Xo, Ref);
    end
 
t = [1:length(y)]' * 0.01;

plot(t,y);
xlabel('Time (seconds)');
ylabel('Ball Position');