%%% Programa: exemplo_5_mraslyapunov.m
%%% Autor: Geovany A. Borges
%%% Objectivo: MRAS para motor CC.
%%% Notas:
%%%

clear all;
close all;
echo off;

FlagImprimir = 0;

FontSize = 10;

%%% Tempo:
TMAX = 10; dt = 0.001; t = [0:dt:TMAX]'; N = length(t);

%%% Simulação do modelo contínuo do motor com falha ocorrendo em t = TMAX/2. 
%%% A falha é dada pela redução de jm a um quarto de seu valor nominal.
rho = 3;
la = 21e-3;  % indutância do rotor
ra = 627e-3; % resistência do rotor
jm = 663e-6; % inércia do rotor
b = 3.1e-3;  % coeficiente de atrito
kt = 31e-3; % indutância do rotor
ke = 1/(la*kt);
ic = 1.0;
A = [-b/jm kt*ic/jm; -kt*ic/la -ra/la];
B = [0; 1/la];

% Modelo de referência:
rho = 0.5;
Am = [-b/(jm*rho) kt*ic/(jm*rho); -kt*ic/(la*rho) -ra/(la*rho)];
Bm = [0; 1/(la)];
Q  = [1 0; 0 1];
P = lyap(Am,Q);
Gama = 0.08;

% Controlador:
M = 0;
L = [0 0];
Theta = zeros(3,1);
Theta0 = [inv(Bm'*B)*Bm'*(A-Am) inv(Bm'*B)*Bm'*Bm]'
%Theta = Theta0;

uc = 10*ones(N,1);
u  = 10*ones(N,1);
X  = zeros(2,N);
Xm = zeros(2,N);
for n=2:N,
   %%% Gerar a entrada de referência:
   if rem(n,round(N/15))==2,
      if uc(n-1)==10
         uc(n) = -10;
      else
         uc(n) =  10;
      end
   else,
      uc(n) = uc(n-1);
   end
   %%% Simular motor usando Euler de primeira ordem em 20 subintervalos
   Xpast = X(:,n-1);
   ndiv = 20;
   for nn=1:ndiv,
      X(:,n) = (eye(2)+dt/20*A)*Xpast + dt/ndiv*B*u(n-1);
      Xpast = X(:,n);
   end
   %%% Simular modelo de referência usando Euler de primeira ordem em 20 subintervalos
   Xpast = Xm(:,n-1);
   ndiv = 20;
   for nn=1:ndiv,
      Xm(:,n) = (eye(2)+dt/20*Am)*Xpast + dt/ndiv*Bm*uc(n-1);
      Xpast = Xm(:,n);
   end
   %%% Erro de modelo
   e(:,n) = X(:,n)-Xm(:,n);
   %%% Simular motor usando Euler de primeira ordem em 20 subintervalos
   Quisi = B*[uc(n) -X(:,n)'];
   Theta = Theta + dt*(-Gama*Quisi'*P*e(:,n));
   %%% Lei de controle:
   M = Theta(1); L = Theta(2:3)'; 
   u(n) = M*uc(n) - L*X(:,n);
end

for dimvar=1:2
      if dimvar==1, str = 'Velocidade de referência (vermelho) e velocidade obtida (azul)'; ylabelstr = '\omega_{ref}, \omega_ [rad/s]'; end
      if dimvar==2, str = 'Corrente de referência (vermelho) e corrente obtida (azul)'; ylabelstr = 'i_{ref}, i [A]'; end
      figure(dimvar); 
      hold on; set(gca,'FontSize',FontSize);
      plot(t,zeros(size(t)),'k:'); 
      plot(t,Xm(dimvar,:),'Color','r','LineStyle','-','Marker','.','MarkerSize',3); 
      plot(t,X(dimvar,:),'Color','b','LineStyle','-','Marker','.','MarkerSize',3); 
      ylabel(ylabelstr,'FontSize',FontSize);
      title(sprintf('%s ',str),'FontSize',FontSize);
      ax = axis; axis([ax(1:2) -max(abs(ax(3:4))) max(abs(ax(3:4)))]);
end

figure(3);
plot(t,u,'LineStyle','-','Marker','.','MarkerSize',3); 
ylabel('u [V]','FontSize',FontSize);
xlabel('t [V]','FontSize',FontSize);
title('Tensão de entrada');

if FlagImprimir
   imprimirtudo('figexemplo_5_1',5);
end
