% Ball & Beam System
% Homework #4

X = [0.01, 0, 0, 0]';
dt = 0.002;
t = 0;
n = 0;
y = [];

while(t < 2)
 Ref = 1;
 U = 0;
 dX = BeamDynamics(X, U);

 X = X + dX * dt;
 t = t + dt;
 
 y = [y ; Ref, X(1)];
 n = mod(n+1,5);
 if(n == 0)
    BeamDisplay(X, Ref);
 end
 end
 
t = [1:length(y)]' * dt;


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