% Gantry System
% Test #2  -  Spring 2024

X = [0,0,0,0]';
Ref = 1;
dt = 0.01;
% Feedback Control Law
Kx = [10,0,10,0];
Kr = 10;
% Full-Order Observer
A = [0,0,1,0;0,0,0,1;0,2.45,0,0;0,-9.42,0,0];
B = [0;0;0.25;-0.1923];
C = [1,0,0,0];
D = 0;
Xe = X;
H = [0,0,0,0]';

t = 0;
n = 0;
y = [];
% Disturbance
d = 0.1;

while( (t < 29.9) & (abs(X(1) < 5) & (abs(Xe(1))<5)))
 Ref = sin(0.5*t);
 U = Kr*Ref - Kx*X;
 
 dX = GantryDynamics(X, U + d);
 dXe = A*Xe + B*U + H*(C*X - C*Xe);
 
 X = X + dX * dt;
 Xe = Xe + dXe * dt;
 
 t = t + dt;
 n = mod(n+1, 5);
 if(n == 0)
    GantryDisplay(X, Xe, Ref);
 end
 y = [y ; X(1), Xe(1), Ref];
end

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