% Cart and Pendulum
% Lecture 26
% LQR with Multiple Inputs

X = [-1,0,0,0]';
Ref = 1;
dt = 0.01;
t = 0;
A = [0,0,1,0 ; 0,0,0,1 ; -0,4.9,0,0 ; 0,14.7,0,0];
% Force Input
BF = [0;0;0.5;-0.5];
% Torque Input
BT = [0;0;-0.5;1.5];
B = [BF, BT];
C = [1,0,0,0 ; 0,1,0,0];

Q = 1e3 * diag([1,0,0,0]);
R = diag([1,1]);
Kx = lqr(A, B, Q, R);
DC = -C*inv(A - B*Kx)*B;
Kr = inv(DC);

n = 0;
y = [];
while(t < 9.9)
 Ref = sign(sin(0.638*t));
 U = Kr*[Ref; 0] - Kx*X;
 dX = CartDynamics(X, U(1), U(2));
 X = X + dX * dt;
 t = t + dt;
 n = mod(n+1, 5);
 if(n == 0)
    CartDisplay(X, Ref);
 end
 y = [y ; X(1), X(2), Ref];
end

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