% Ball & Beam System
% Test #2 (Spring 2025)
 
X = [-1,0,0,0]';
dt = 0.01;
t = 0;
n = 0;
y = [];
 
% Plant
A = [0,0,1,0;0,0,0,1;0,-7,0,0;-7.434,0,0,0];
B = [0;0;0;0.345];
C = [1,0,0,0];
 
% Control Law  (needs to change)
Kx = [-31   101   -21    29];
Kr = -21;
 
% Full-Order Observer (needs to change)
Ae = A;
Be = B;
Ce = C;
H = [0 0 0 0]';
Xe = X;

% Ramp Input
dR = dt / pi;
Ref = -1;
 
while(t < 40)
 Ref = Ref + dR;
 if(abs(Ref) > 1) dR = -dR; end
 
 U = Kr*Ref - Kx*X;
 
 dX = BeamDynamics(X, U);
 dXe = Ae*Xe + Be*U + H*(C*X - Ce*Xe);
 
 X = X + dX * dt;
% Observer (cheating for now)
 Xe = X + [0.1,0,0,0]';
 t = t + dt;
 
 y = [y ; Ref, X(1)];
 n = mod(n+1,5);
 if(n == 0)
    BeamDisplay3(X, Xe, Ref);
 end
 end
 
t = [1:length(y)]' * dt;
 
 
plot(t,y(:,1),'r',t,y(:,2),'b');
xlabel('Time (seconds)');
ylabel('Ball Position');