% Test #3 for Spring 2024
% Double pendulum with first link pointing up
% second link pointing down

X = [-1, 0, 0, 0, 0, 0]';
Ref = 1;
dt = 0.01;
U = 0;
t = 0;

% Plant Dynamics
Z = zeros(3,3);
I = eye(3,3);
K = [0,0,0;0,3,-1;0,1,-1];
M = [3,1,-1;1,1,0;-1,0,1];
A6 = [Z,I;inv(M)*K*9.8,Z];
B6 = [Z;inv(M)];
C6 = [1,0,0,0,0,0;0,1,0,0,0,0;0,0,1,0,0,0];

% Starting Control Law (only uses force)
% Need to use all three inputs (F, T1, T2) for the test

 Kx = ppl(A6, B6(:,1), [-1,-2,-3,-4,-5,-6]);
 K6 = [Kx ; 0,0,0,0,0,0 ; 0,0,0,0,0,0];
 DC = -C6*inv(A6-B6*K6)*B6;
 Kr = inv(DC);
 
 % Set point: [x=Ref ; q1=0 ; q2=0]
 
Data = [];

while(t < 5)
 Ref = 1;
 U = Kr*[Ref;0;0] - K6*X;
 dX = Cart2Dynamics(X, U(1), U(2), U(3));
  
%plant
 X = X + dX * dt;
% observer
 Xe = X + [0.1;0;0;0;0;0];
 
 t = t + dt;
 Cart2Display(X, Xe, Ref);
 plot([Ref, Ref],[-0.1,0.1],'b');
 Data = [Data ; X(1), X(2), X(3), Xe(1), Xe(2), Xe(3), Ref];
end

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