%%Definir tamanho da fonte dos eixos e da legenda dos eixos para todas as imagens:
set(groot, 'DefaultAxesFontSize',12)
set(groot, 'DefaultAxesLabelFontSizeMultiplier', 1.4)
%%Definir o LaTeX como interpretador para os eixos e suas respectivas legendas:
set(groot, 'DefaultTextInterpreter','LaTeX')  
set(groot, 'DefaultAxesTickLabelInterpreter','LaTeX')

%% Modelo Não - Linear

[t1,x] = ode45(@invertido,[0 0.8], [0;0;0;0]);

figure(1);
plot(t1,x(:,1),'red')
xlabel('$t(s)$')
ylabel('$x(m)$')
grid on

figure(2);
plot(t1,x(:,3)*180/pi,'red')
xlabel('$t(s)$')
ylabel('$\theta (^\circ)$')
grid on

%% Modelo Linear

% Parâmetros
r = 1;      % Força Degrau (N)
M = 2;      % Massa do carrinho maior (Kg)
m = 0.1;    % Massa do carrinho menor (Kg)
l = 0.5;    % Comprimento do pêndulo (m)
g = 9.81;   % Gravidade local (m/s^2)

A = [0 1 0 0; 0 0 -m*g/M 0; 0 0 0 1; 0 0 (M+m)*g/(M*l) 0];
B = [0 ; 1/M; 0 ; -1/(M*l)];
C = [1 0 0 0; 0 0 1 0];
D = zeros(2,1);
G = ss(A,B,C,D);
t = [0:0.1/2:0.8];
y = step(t,G);

figure(3);
plot(t1,x(:,1),'black',t,y(:,1),'or')
xlabel('$t(s)$')
ylabel('$x(m)$')
grid on
legend('Não Linear','Linear')

figure(4);
plot(t1,x(:,3)*180/pi,'black',t,y(:,2)*180/pi,'or')
grid on
xlabel('$t(s)$')
ylabel('$\theta (^\circ)$')
legend('Não Linear','Linear')

%% Controle Por Realimentação de Estados - Escolha dos Polos
P1 = [-2 -1+10*1j -1-10*1j -1];    
K1 = place(A,B,P1);

sim('sel0328_simulacao',6)

figure(5);
plot(a.time, a.signals.values,c.time, c.signals.values,e.time, e.signals.values,g.time, g.signals.values)
leg = legend('$-45^\circ$', '$-15^\circ$','$  15^\circ$','$  45^\circ$');
set(leg,'Interpreter','latex');
set(leg,'FontSize',12);
grid on
ylabel('$x(m)$')
xlabel('$t(s)$')

figure(6);
plot(b.time, b.signals.values*180/pi,d.time, d.signals.values*180/pi,f.time, f.signals.values*180/pi,h.time, h.signals.values*180/pi)
leg = legend('$-45^\circ$', '$-15^\circ$','$15^\circ$','$45^\circ$');
set(leg,'Interpreter','latex');
set(leg,'FontSize',12);
xlabel('$t(s)$')
ylabel('$\theta ^\circ$')
grid on


%% Controle Por Realimentação de Estados - LQR
%LQR1
Q1 = [8 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
R1 = 1;
[K2,P2] = lqr(A,B,Q1,R1);
sim('sel0328_simulacao1',6)

figure(7);
plot(a1.time, a1.signals.values, c1.time, c1.signals.values, e1.time, e1.signals.values,g1.time, g1.signals.values)
leg = legend('$-45^\circ$', '$-15^\circ$','$  15^\circ$','$  45^\circ$');
set(leg,'Interpreter','latex');
set(leg,'FontSize',12);
grid on
ylabel('$x(m)$')
xlabel('$t(s)$')

figure(8);
plot(b1.time, b1.signals.values*180/pi,d1.time, d1.signals.values*180/pi,f1.time, f1.signals.values*180/pi,h1.time, h1.signals.values*180/pi)
leg = legend('$-45^\circ$', '$-15^\circ$','$15^\circ$','$45^\circ$');
set(leg,'Interpreter','latex');
set(leg,'FontSize',12);
xlabel('$t(s)$')
ylabel('$\theta ^\circ$')
grid on

%LQR2
Q2 = [250 0 0 0; 0 210 0 0;0 0 1 0; 0 0 0 1];
R2 = 0.3;
[K3,P3] = lqr(A,B,Q2,R2);
sim('sel0328_simulacao2',6)
figure(9);
plot(a2.time, a2.signals.values, c2.time, c2.signals.values, e2.time, e2.signals.values,g2.time, g2.signals.values)
leg = legend('$-45^\circ$', '$-15^\circ$','$  15^\circ$','$  45^\circ$');
set(leg,'Interpreter','latex');
set(leg,'FontSize',12);
grid on
ylabel('$x(m)$')
xlabel('$t(s)$')

figure(10);
plot(b2.time, b2.signals.values*180/pi,d2.time, d2.signals.values*180/pi,f2.time, f2.signals.values*180/pi,h2.time, h2.signals.values*180/pi)
leg = legend('$-45^\circ$', '$-15^\circ$','$15^\circ$','$45^\circ$');
set(leg,'Interpreter','latex');
set(leg,'FontSize',12);
xlabel('$t(s)$')
ylabel('$\theta ^\circ$')
grid on