% EESC/USP - SEM 538 - Sistemas de Controle II
% Exercício 1 - 2016
%

close all
clear all

m = 1;
l = 1;
a = 0.25;
g = 10;
k = 10;


%Espaço de Estados
A = [0      1     0    0;
    -(((k*a*a)/(m*l*l))+(g/l)) 0 ((k*a*a)/(m*l*l)) 0;
    0       0     0    1;
    ((k*a*a)/(m*l*l)) 0 -(((k*a*a)/(m*l*l))+(g/l)) 0];

B=[0; -1/(m*l); 0; 1/(m*l)];

C=[1 0 0 0; 0 0 1 0];

D=[0; 0];
S = ss(A,B,C,D);
 
%Respota ao impulso
figure
[y,t] = impulse(S,10);
plot(t,y)
legend ('\theta_1','\theta_2')

figure
alfa = y(:,1) + y(:,2);
beta = y(:,1) - y(:,2);
plot(t,alfa,'b');hold on;plot(t,beta,'g')
legend ('Alfa','Beta')

pause
figure
for k = 1:length(y),
    
    
    clf
    subplot(1,2,1)
    for dl=0:l/20:l
        hold on
        plot(dl*l*cos(y(k,1)-pi/2),dl*l*sin(y(k,1)-pi/2),'.r','LineWidth',1)
        plot(l*cos(y(k,1)-pi/2),l*sin(y(k,1)-pi/2),'O','LineWidth',1)
        xlabel('pendulum 1')
        ylim([-1.1 0])
        xlim([-0.4 0.4])
        hold off
    end
    
    subplot(1,2,2)
    for dl=0:l/20:l
        hold on
        plot(dl*l*cos(y(k,2)-pi/2),dl*l*sin(y(k,2)-pi/2),'.r','LineWidth',1)
        plot(l*cos(y(k,2)-pi/2),l*sin(y(k,2)-pi/2),'O','LineWidth',1)
        xlabel('pendulum 2')
        ylim([-1.1 0])
        xlim([-0.4 0.4])
        hold off
    end
    pause(0.025)
    
end
pause

% Resposta condição inicial
figure
X0 = [-.2;0;.1;0];
[y,t] = initial(S,X0,10);

plot(t,y)
legend ('\theta_1','\theta_2')

figure
alfa = y(:,1) + y(:,2);
beta = y(:,1) - y(:,2);
plot(t,alfa,'b');hold on;plot(t,beta,'g')
legend ('Alfa','Beta')

pause
figure
for k = 1:length(y),
    
    
    clf
    subplot(1,2,1)
    for dl=0:l/20:l
        hold on
        plot(dl*l*cos(y(k,1)-pi/2),dl*l*sin(y(k,1)-pi/2),'.r','LineWidth',1)
        plot(l*cos(y(k,1)-pi/2),l*sin(y(k,1)-pi/2),'O','LineWidth',1)
        xlabel('pendulum 1')
        ylim([-1.1 0])
        xlim([-0.2 0.2])
        hold off
    end
    
    subplot(1,2,2)
    for dl=0:l/20:l
        hold on
        plot(dl*l*cos(y(k,2)-pi/2),dl*l*sin(y(k,2)-pi/2),'.r','LineWidth',1)
        plot(l*cos(y(k,2)-pi/2),l*sin(y(k,2)-pi/2),'O','LineWidth',1)
        xlabel('pendulum 2')
        ylim([-1.1 0])
        xlim([-0.2 0.2])
        hold off
    end
    pause(0.025)
    
end

pause

%Controle
Q=eye(4)*10;
R=1;
K = lqr(S,Q,R)

%K = [1 1 1 1];
S2 = ss(A-B*K,B,C,D);
X0 = [-.2;0;.1;0];
[y,t] = initial(S2,X0,6);

figure
plot(t,y)
legend ('\theta_1','\theta_2')

figure
alfa = y(:,1) + y(:,2);
beta = y(:,1) - y(:,2);
plot(t,alfa,'b');hold on;plot(t,beta,'g')
legend ('Alfa','Beta')

pause
figure
for k = 1:length(y),
    
    
    clf
    subplot(1,2,1)
    for dl=0:l/20:l
        hold on
        plot(dl*l*cos(y(k,1)-pi/2),dl*l*sin(y(k,1)-pi/2),'.r','LineWidth',1)
        plot(l*cos(y(k,1)-pi/2),l*sin(y(k,1)-pi/2),'O','LineWidth',1)
        xlabel('pendulum 1')
        ylim([-1.1 0])
        xlim([-0.2 0.2])
        hold off
    end
    
    subplot(1,2,2)
    for dl=0:l/20:l
        hold on
        plot(dl*l*cos(y(k,2)-pi/2),dl*l*sin(y(k,2)-pi/2),'.r','LineWidth',1)
        plot(l*cos(y(k,2)-pi/2),l*sin(y(k,2)-pi/2),'O','LineWidth',1)
        xlabel('pendulum 2')
        ylim([-1.1 0])
        xlim([-0.2 0.2])
        hold off
    end
    pause(0.025)
    
end

pause

% Sistema Controlável
B=[0 0; -1/(m*l) 0; 0 0; 0 1/(m*l)];
D= zeros(2);
S3 = ss(A,B,C,D);

Q=eye(4)*10;
R=eye(2);
K = lqr(S3,Q,R);

S4 = ss(A-B*K,B,C,D);
X0 = [-.2;0;.1;0];
[y,t] = initial(S4,X0,6);

figure
plot(t,y)
legend ('\theta_1','\theta_2')

figure
alfa = y(:,1) + y(:,2);
beta = y(:,1) - y(:,2);
plot(t,alfa,'b');hold on;plot(t,beta,'g')
legend ('Alfa','Beta')

pause
figure
for k = 1:length(y),
    
    
    clf
    subplot(1,2,1)
    for dl=0:l/20:l
        hold on
        plot(dl*l*cos(y(k,1)-pi/2),dl*l*sin(y(k,1)-pi/2),'.r','LineWidth',1)
        plot(l*cos(y(k,1)-pi/2),l*sin(y(k,1)-pi/2),'O','LineWidth',1)
        xlabel('pendulum 1')
        ylim([-1.1 0])
        xlim([-0.2 0.2])
        hold off
    end
    
    subplot(1,2,2)
    for dl=0:l/20:l
        hold on
        plot(dl*l*cos(y(k,2)-pi/2),dl*l*sin(y(k,2)-pi/2),'.r','LineWidth',1)
        plot(l*cos(y(k,2)-pi/2),l*sin(y(k,2)-pi/2),'O','LineWidth',1)
        xlabel('pendulum 2')
        ylim([-1.1 0])
        xlim([-0.2 0.2])
        hold off
    end
    pause(0.025)
    
end