%--------------- RLQR Aplicado no pendulo invertido ----------------------%
%Mauricio Escalante Ortega N°USP 9173278

warning('off','all')
warning
clc, clear all, close all

tempo_inicio = cputime;
disp('Horas de simulação:')
total = (cputime - tempo_inicio)/3600

Ts=0.0001;
dados_pendulo_RNA;



%-------------------------MODELO INCERTO----------------------------------%
l_in=0;  
M_in=0;  
m_in=0; 

Ia_in=Ia*7; 
b_in=0;   
Ka_in=10; 

sys_in=modelo_pendulo_lineal_incertezas_R(l_in,M_in,m_in,Ia_in,b_in, Ka_in);
sysd_in=c2d(sys_in,Ts);
F_in=sysd_in.a;
G_in=sysd_in.b;
%-------------------------------------------------------------------------%
% M. Discreto
sys=modelo_pendulo_lineal_R();
sysd=c2d(sys,Ts);
F=sysd.a;
G=sysd.b;

%---------------------Matrices de Ponderação------------------------------% 

 R=eye(2);
 Q=eye(4);
 P(:,:,1) = Q*1e10;
 

%-------------------------Matrices de Incerteza---------------------------%                             %%%


H=[1 1 1 1]';
Ef=[1 1 1 1]*10^(-6);  
Eg=[5*10^(-7) 5*10^(-7)];            
 


N = 50000; % Horizonte


clear x
clear u
x(:,1) = [ 0 ; 0 ; 0 ; 0];
alpha=2;
mu=4e19;



            for cont_control=1:N
                [L,K,P] = robust_control(P,R,Q,F,G,Ef,Eg);
                
            end
            
% - - - - - - - - - - - - - - %  Simulação  % - - - - - - - - - - - - - - %


N=2*150000;
r_max=0.35;
Tr=30; % Tempo sinal de referência
pulse = -gensig('square',Tr,(N-1)*Ts,Ts)+0.5;
 ref=r_max*pulse*2;
 
 Gref=0.01; 
 
    for k = 1:N
         u(:,k) =K * x(:,k);
         x_hat(:,k+1) = L * x(:,k)+ [0 0 0 0.01]'*ref(k); 
         x(:,k+1) = F * x(:,k) + G* u(:,k)+Gref*[0 0 0  1]' *ref(k);                
    end
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% - - - - - - - - - - - - - - %   Grafico  % - - - - - - - - - - - - - - %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(1)

subplot(3,1,1)
plot((1:N)*Ts,x(1,1:N),'--','LineWidth',1)
hold on
title('Ângulo')
grid on

subplot(3,1,2)
plot((1:N)*Ts,x(2,1:N),'--','LineWidth',1)
hold on
plot((1:N)*Ts,x_hat(2,1:N),'r--','LineWidth',2)
plot((1:N)*Ts,ref(1:N),'black','LineWidth',1)
%hold off
title('Deslocamento')
grid on

subplot(3,1,3)
plot((1:N)*Ts,u(1,:),'--')
hold on
title('Sinal de controle')
grid on
           

disp('horas de simulacion:')
total = (cputime - tempo_inicio)/3600