% Ball & Beam System
% homework #6: pole placement

X = [0,0,0,0]';
dt = 0.01;
t = 0;
Kx = [ -10   20   -10   20 ];
Kr = -2;
y = [];
n = 0;

while(t < 20)
 Ref = 1;
 U = Kr*Ref - Kx*X;
 dX = BeamDynamics(X, U);
  
 X = X + dX * dt;
 t = t + dt;
 
 n = mod(n+1, 5);
 if(n == 0)
    y = [y ; Ref, X(1)];
    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');