% Cart and Pendulum ( version)
% Homework #6: pole placement

X = zeros(4,1);
dX = zeros(4,1);
Ref = 1;
dt = 0.01;
t = 0;
Kx = [-1  -70   -5  -15];
Kr =  -1;

n=0;

y = [];
while(t < 15)
   Ref = 1;
   U = Kr*Ref - Kx*X;
   dX = CartDynamics(X, U);

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

t = [1:length(y)]' * dt;
plot(t,y);