% Gantry Main Routine
% ECE 463 lecture #7

X = [-1;0;0;0];
dX = zeros(4,1);
Ref = 0;
dt = 0.025;
U = 0;
t = 0;

% Full State Feedback
Kx = [ 10  0   10   0];
Kr = 10;

y = [];

while(t < 12)
 Ref = sign(sin(0.15*pi*t));
 U = Kr*Ref - Kx*X;
 dX = GantryDynamics(X, U);
 
 X = X + dX * dt;
 
 t = t + dt;
 
 GantryDisplay(X, Ref);
 plot([Ref, Ref],[-0.1,0.1],'b');
 
 y = [y ; X(1) Ref];
 
end

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