% Cart and Pendulum
% Homework #4: Dynamics

X = [-1,0.001,0,0]';
Ref = 1;
dt = 0.01;
t = 0;
n = 0;
y = [];

while(t < 9.9)
 Ref = sign(sin(0.638*t));
 U = 0;
 dX = CartDynamics(X, U);
 X = X + dX * dt;
 t = t + dt;
 n = mod(n+1, 5);
 if(n == 0)
    CartDisplay(X, Ref);
 end
 y = [y ; X(1), X(2), Ref];
end

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