% ECE 463/663 Final Exam
% Cart with two pendulums

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

Kx = [10  0 0 20 0 0 ];
Kr = 10;

while(t < 29)
 Ref = 1*(sin(0.314*t) > 0);
 U = Kr*Ref - Kx*X;
 
 dX = Cart2Dynamics(X, U + 1);
 
 X = X + dX * dt;
 
 t = t + dt;
 n = mod(n+1, 5);
 if(n == 0)
    Cart2Display(X, Ref);
 end
 y = [y ; X(1), Ref];
end

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