%Integral Control

clear all
clc

a=10;
b=1;
c=10;

a_p=10;
b_p=0.5;
c_p=5;

delta=pi/4;

Tss=a*sin(delta)/c

thetass=delta

A=[0 1; -a*cos(delta) -b];
B=[0; c];
C=[1 0];


%PROPORTIONAL CONTROL

Pp=[-4+i*4.59 -4-i*4.59]

Kp = place(A,B,Pp)

% Test1=b+c*Kp(2)
% Test2=a*cos(delta)+c*Kp(1)
% 
% Test1_p=b_p+c_p*Kp(2)
% Test2_p=a_p*cos(delta)+c_p*Kp(1)

Y0=[-pi/4;0];
Tfinal=3;
TSPAN=[0 Tfinal];

[T,Y]=ode45(@(t,y) Pendulum_fun1(t,y,a,b,c,delta,Tss,Kp),TSPAN,Y0);

theta1=Y(:,1)+delta;
dtheta1=Y(:,2);
u1=-Kp*Y';
Torque1=u1+Tss;
Y1=Y;
T1=T;

[T_p,Y_p]=ode45(@(t,y) Pendulum_fun1(t,y,a_p,b_p,c_p,delta,Tss,Kp),TSPAN,Y0);

theta1_p=Y_p(:,1)+delta;
dtheta1_p=Y_p(:,2);
u1_p=-Kp*Y_p';
Torque1_p=u1_p+Tss;
Y1_p=Y_p;
T1_p=T_p;

figure(1)
subplot(2,3,1)
plot(T1,theta1,'b-',T1_p,theta1_p,'r:')
axis([0 3 0 1])
xlabel('Time [s]')
ylabel('\theta')
subplot(2,3,2)
plot(T1,dtheta1,'b-',T1_p,dtheta1_p,'r:')
axis([0 3 -0.5 3])
xlabel('Time [s]')
ylabel('d\theta/dt')
subplot(2,3,3)
plot(T1,Torque1,'b-',T1_p,Torque1_p,'r:')
axis([0 3 0 3.5])
xlabel('Time [s]')
ylabel('Torque')
subplot(2,3,4)
plot(T1,Y1(:,1),'b-',T1_p,Y1_p(:,1),'r:')
axis([0 3 -0.8 0.2])
xlabel('Time [s]')
ylabel('x_1')
subplot(2,3,5)
plot(T1,Y1(:,2),'b-',T1_p,Y1_p(:,2),'r:')
axis([0 3 -0.5 2.5])
xlabel('Time [s]')
ylabel('x_2')
subplot(2,3,6)
plot(T1,u1,'b-',T1_p,u1_p,'r:')
axis([0 3 -1 3])
xlabel('Time [s]')
ylabel('u')


%INTEGRAL CONTROL

Aaug=[A zeros(size(A,1),1);C zeros(size(C,1),1)];
Baug=[B; 0];

Pi=[-15.93 -2.93 -2.14]

Ki = place(Aaug,Baug,Pi)

% Test1i=b+c*Ki(2)
% Test2i=a*cos(delta)+c*Ki(1)
% Test3i=c*Ki(3) 
% 
% Test1i_p=b_p+c_p*Ki(2)
% Test2i_p=a_p*cos(delta)+c_p*Ki(1)
% Test3i_p=c_p*Ki(3) 

Y0=[-pi/4;0;0];
Tfinal=3;
TSPAN=[0 Tfinal];
[T,Y]=ode45(@(t,y) Pendulum_fun2(t,y,a,b,c,delta,Tss,Ki),TSPAN,Y0);

theta2=Y(:,1)+delta;
dtheta2=Y(:,2);
u2=-Ki*Y';
Torque2=u2+Tss;
Y2=Y;
T2=T;

[T_p,Y_p]=ode45(@(t,y) Pendulum_fun2(t,y,a_p,b_p,c_p,delta,Tss,Ki),TSPAN,Y0);

theta2_p=Y_p(:,1)+delta;
dtheta2_p=Y_p(:,2);
u2_p=-Ki*Y_p';
Torque2_p=u2_p+Tss;
Y2_p=Y_p;
T2_p=T_p;

figure(2)
subplot(2,3,1)
plot(T2,theta2,'b-',T2_p,theta2_p,'r:')
axis([0 3 0 1])
xlabel('Time [s]')
ylabel('\theta')
subplot(2,3,2)
plot(T2,dtheta2,'b-',T2_p,dtheta2_p,'r:')
axis([0 3 -0.5 3])
xlabel('Time [s]')
ylabel('d\theta/dt')
subplot(2,3,3)
plot(T2,Torque2,'b-',T2_p,Torque2_p,'r:')
axis([0 3 0 3.5])
xlabel('Time [s]')
ylabel('Torque')
subplot(2,3,4)
plot(T2,Y2(:,1),'b-',T2_p,Y2_p(:,1),'r:')
axis([0 3 -0.8 0.2])
xlabel('Time [s]')
ylabel('x_1')
subplot(2,3,5)
plot(T2,Y2(:,2),'b-',T2_p,Y2_p(:,2),'r:')
axis([0 3 -0.5 2.5])
xlabel('Time [s]')
ylabel('x_2')
subplot(2,3,6)
plot(T2,u2,'b-',T2_p,u2_p,'r:')
axis([0 3 -1 3])
xlabel('Time [s]')
ylabel('u')

figure(3)
plot(T2,Y2(:,3),'b-',T2_p,Y2_p(:,3),'r:')
axis([0 3 -0.2 0])
xlabel('Time [s]')
ylabel('\sigma')

