%%% Arquivo: exemplo_11_1.m
%%% Autor: G. A. Borges
%%% Objetivo: Controle LQ de motor de corrente continua acionado por corrente.
echo off; clear all; close all; format long;

FlagImprimir = 0;

% Amostragem: 
T = 0.05;
t = [0:T:6]';
N = length(t);
tf = 3;

% Parâmetros do motor:
a = exp(-T);
F = [a 0; 1-a 1];
G = [1-a; T-1+a];

% Parâmetros do controlador ótimo:
Q2caso = [1; 10; 200];
Q0caso = zeros(2,2,3);
Q0caso(:,:,1) = diag([1.0 0.0]);
Q0caso(:,:,2) = diag([0.1 0.0]);
Q0caso(:,:,3) = diag([0.001 0.0]);
legenda = repmat(struct('strg',''),3,1);
for ncaso=1:2,
	entrada = zeros(N,3);
	vel = zeros(N,3); 
	pos = zeros(N,3);
   for nvalor=1:3
      % Matrizes:
      if ncaso==1,
			Q1 = diag([1.0 0.0]);
         Q2 = Q2caso(nvalor);
         Q0 = Q0caso(:,:,1);
      end
      if ncaso==2,
			Q1 = diag([0.0 0.0]);
         Q2 = 0.001;
         Q0 = Q0caso(:,:,nvalor);
      end
      legenda(nvalor).strg = sprintf('\\rho_0 = %f, \\rho_1 = %f, \\rho_2 = %f',Q0(1,1),Q1(1,1),Q2);
      
      % Solução da equação de riccati:
      Ni = 1; Nf = round(tf/T);
      S = zeros(2,2,N);
      L = zeros(1,2,N);
      S(:,:,Nf) = Q0;
      for k=Nf-1:-1:1,
         L(:,:,k) = inv(Q2+G'*S(:,:,k+1)*G)*G'*S(:,:,k+1)*F;
         S(:,:,k) = (F-G*L(:,:,k))'*S(:,:,k+1)*(F-G*L(:,:,k)) + Q1 + L(:,:,k)'*Q2*L(:,:,k);
      end
      
      % Realizar o controle:
      v0 = 10;
      p0 = 0;
      x = zeros(2,N);
      u = zeros(1,N);
      x(1,1) = v0;
      x(2,1) = p0;
      for k=1:N,
         if k>1,
            % Lei de controle:
            u(:,k-1) = -L(:,:,k-1)*x(:,k-1);
            % Modelo do motor:
            x(:,k) = F*x(:,k-1) + G*u(:,k-1);
         end
      end
      % Salvar variaveis:
      vel(:,nvalor) = x(1,:)';
      pos(:,nvalor) = x(2,:)';
      entrada(:,nvalor) = u(1,:)';         
   end
   
   % Imprimir:
   figure;
   plot(t,vel,'-','LineWidth',2); hold on;
   ax = axis; plot([tf tf],[ax(3) ax(4)],'r:');
   xlabel('t [s]'); ylabel('velocidade [rad/s]');
   set(gca,'PlotBoxAspectRatio',[1 0.5 1]);
   legend(legenda(1).strg,legenda(2).strg,legenda(3).strg);
   
   figure;
   plot(t,pos,'-','LineWidth',2); hold on;
   ax = axis; plot([tf tf],[ax(3) ax(4)],'r:');
   xlabel('t [s]'); ylabel('posicao [rad]');
   set(gca,'PlotBoxAspectRatio',[1 0.5 1]);
   legend(legenda(1).strg,legenda(2).strg,legenda(3).strg);
   
   figure;
   plot(t,entrada,'-','LineWidth',2); hold on;
   ax = axis; plot([tf tf],[ax(3) ax(4)],'r:');
   xlabel('t [s]'); ylabel('entrada u');
   set(gca,'PlotBoxAspectRatio',[1 0.5 1]);
   legend(legenda(1).strg,legenda(2).strg,legenda(3).strg);
end


if FlagImprimir
   disp('Posicione as legendas e pressione alguma tecla para imprimir');
   pause;
   imprimirtudo('figexemplo_11_1',6);
end
