% Ball & Beam System
% Sp 19 Version
% Homework #7 (observer)
% m = 1kg
% J = 2 kg m^2

X = [0, 0, 0, 0]';
dt = 0.01;
t = 0;
Kx = [  -13.8052   60.6061   -9.7403   22.7273];
Kr = -3.8961;

y = [];

while(t < 20)
 Ref = 1 * (sin(0.1*pi*t) > 0);
 
 U = Kr*Ref - Kx*Xo;

 dX = BeamDynamics(X, U);
 
 % cheating:  observer exactly follows the plant
 Xo = X;
  
 X = X + dX * dt;
 Xo = Xo + dXo * dt;
 
 y = [y ; Ref, X(1), Xo(1)];
 
 t = t + dt;
 Beam7Display(X, Xo, Ref);
 end
 
t = [1:length(y)]' * dt;


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