% Ball & Beam System
% Sp 19 Version
% m = 1kg
% J = 2 kg m^2

X = [0, 0, 0, 0]';
dt = 0.01;
t = 0;
Kx = [-11.6501   35.1489   -4.1042   18.0018];
Kr = -1.8391;

y = [];

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

 dX = BeamDynamics(X, U);
 dZ = X(1) - Ref;
 
 X = X + dX * dt;

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


plot(t,y(:,1),'r',t,y(:,2),'b');
xlabel('Time (seconds)');
ylabel('Ball Position');