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

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

g = 9.8;
a1 = [0,0,0,1,0,0]
a2 = [0,0,0,0,1,0];
a3 = [0,0,0,0,0,1];
a4 = [0,-2*g,0,0,0,0];
a5 = [0,3*g,-g,0,0,0];
a6 = [0,-3*g,3*g,0,0,0];
A = [a1;a2;a3;a4;a5;a6];
Bf = [0;0;0;1;-1;1];  % force input
Bt = [0;0;0;-1;2;-3]; % T1 input
B = [Bf,Bt];
C = [1,0,0,0,0,0;0,1,0,0,0,0;0,0,1,0,0,0];
D = zeros(3,2);

% pole placement to find force
Kf = ppl(A, Bf, [-1,-2,-3,-4,-5,-6]);
% set T1 = 0 (Kx = 0 for the torque input)
Kt = zeros(1,6);
%Kx is a 6x2 matrix (6 states, 2 inputs)
Kx = [Kf ; Kt];

DC = -C*inv(A - B*Kx)*B;
Kr = [1/DC(1,1); 0];

Ae = A;
Be = B;
Ce = C;
Xe = X;

while(t < 20)
 Ref = sign(sin(0.314*t));
 U = Kr*Ref - Kx*X;
 
 %dX = Cart2Dynamics(X, F, T1)
 dX = Cart2Dynamics(X, U(1), U(2));
 dZ = X(1) - Ref;
 dXe = 0;
 
 X = X + dX * dt;
 Z = Z + dZ * dt;
 Xe = X + [0.1;0;0;0;0;0];
 
  
 t = t + dt;
 n = mod(n+1, 5);
 if(n == 0)
    Cart2Display(X, Xe, Ref);
 end
 y = [y ; X(1), X(1), X(3), Ref];
end

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