%Single Link Inverted Pendulum Control
clear all;
close all;
global A B C D

%Single Link Inverted Pendulum Parameters
g=9.8;
M=1.0;
m=0.1;
L=1.0;
Fc=0.0005;

I=1/12*m*L^2;  
l=1/2*L;
t1=m*(M+m)*g*l/[(M+m)*I+M*m*l^2];
t2=-m^2*g*l^2/[(m+M)*I+M*m*l^2];
t3=-m*l/[(M+m)*I+M*m*l^2];
t4=(I+m*l^2)/[(m+M)*I+M*m*l^2];

A=[0,1,0,0;
   t1,0,0,0;
   0,0,0,1;
   t2,0,0,0];
B=[0;t3;0;t4];
C=[1,0,0,0;
   0,0,1,0];
D=[0;0];

Q=[100,0,0,0;   %100,10,1,1 express importance of theta,dtheta,x,dx
   0,10,0,0;
   0,0,1,0;
   0,0,0,1];
R=[0.1];
K=LQR(A,B,Q,R); %LQR Gain    

e1_1=0;e2_1=0;e3_1=0;e4_1=0;
u_1=0;
xk=[0.2,0,0.5,0];   %Initial state

ts=0.02;
for k=1:1:1000
time(k)=k*ts;
Tspan=[0 ts];

para=u_1;
[t,x]=ode45('chap7_12f',Tspan,xk,[],para);
xk=x(length(x),:);

r1(k)=0.0;    %Pendulum Angle
r2(k)=0.0;    %Pendulum Angle Rate
r3(k)=0.0;    %Car Position
r4(k)=0.0;    %Car Position Rate

x1(k)=xk(1);
x2(k)=xk(2);
x3(k)=xk(3);
x4(k)=xk(4);

e1(k)=r1(k)-x1(k);
e2(k)=r2(k)-x2(k);
e3(k)=r3(k)-x3(k);
e4(k)=r4(k)-x4(k);

S=2;
if S==1      %LQR
	u(k)=K(1)*e1(k)+K(2)*e2(k)+K(3)*e3(k)+K(4)*e4(k);
elseif S==2  %PD
   de1(k)=e1(k)-e1_1;
   u1(k)=-50*e1(k)-10*de1(k);
   de2(k)=e2(k)-e2_1;
   u2(k)=-10*e2(k)-10*de2(k);
   de3(k)=e3(k)-e3_1;
   u3(k)=-10*e3(k)-10*de3(k);
   de4(k)=e4(k)-e4_1;
   u4(k)=-10*e4(k)-10*de4(k);
   u(k)=u1(k)+u2(k)+u3(k)+u4(k);
end


	e1_1=e1(k);
	e2_1=e2(k);
	e3_1=e3(k);
	e4_1=e4(k);
	u_1=u(k);
end
figure(1);
subplot(411);
plot(time,r1,'k',time,x1,'k');      %Pendulum Angle
xlabel('time(s)');ylabel('Angle');
subplot(412);
plot(time,r2,'k',time,x2,'k');      %Pendulum Angle Rate
xlabel('time(s)');ylabel('Angle rate');
subplot(413);
plot(time,r3,'k',time,x3,'k');      %Car Position
xlabel('time(s)');ylabel('Cart position');
subplot(414);
plot(time,r4,'k',time,x4,'k');      %Car Position Rate
xlabel('time(s)');ylabel('Cart rate');
figure(5);
plot(time,u,'k');  %Force F change
grid on;
xlabel('time(s)');ylabel('Force');