clear all
close all

% Definicion de los parametros de simulacion
fm=100;                 % FRECUENCIA DE MUESTREO
Tm=1/fm;                % PERIODO DE MUESTREO
Tf=40;                  % TIEMPO TOTAL DE SIMULACION
na=Tf/Tm;                % NUMERO DE TOTAL DE MUESTRAS EN LA SIMULACION
p=1;                    % NUMERO DE PUNTOS POR CADA PERIODO DE MUESTREO (PASO DE SIMULACION)
np=na*p;                % NUMERO TOTAL DE PUNTOS DE LA SIMULACION

% Valores base para normalización de la planta a valores en p.u.
Wbase=2000*(2*pi/60);   % VALOR BASE DE VELOCIDAD ANGULAR PARA NORMALIZACION (VELOCIDAD NOMINAL) EN RAD/SEG
Ibase=10;               % VALOR BASE DE CORRIENTE PARA NORMALIZACION (VALOR CC NOMINAL) EN AMPERES
Vbase=220;              % VALOR DE LA FUENTE DE TENSION CC DE ENTRADA EN VOLTIOS
Wref=2000*(2*pi/60);    % VALOR DE REFERENCIA DE VELOCIDAD ANGULAR (VELOCIDAD NOMINAL) EN RAD/SEG
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETROS DE LA PLANTA.
La=5e-3;                % Indutancia de la armadura en mH
Ra=0.5;              	% Resistencia de armadura en Ohms
Kb=0.8;                 % Coeficiente de velocidad en V/rad/seg
Kt=1;                   % Coeficiente de torque en N.m/A
J=2;                    % Momento de Inercia en N.M/rad/seg^2
b=0.05;                 % Coeficiente de rozamiento en N.m.seg/rad
T=Tm;

% Definición de las matrices de estado del sistema
% x1=w (velocidad angular), x2=ia (corriente de armadura)  
A=[ -b/J     Kt/J;
   -Kb/La  -Ra/La];
B=[0;1/La];
F=[-1/J;0];
C=[1 0]; D=0;

% Determinación de la controlabilidad del sistema
rango_planta=rank(ctrb(A,B));        % rango debe ser igual al orden del sistema

% Discretizacion de las matrices para simulación de la planta en tiempo continuo
% con periodo de muestreo=T/p. Planta sin normalizar
[Gs,Hs]=c2d(A,B,T/p);
[Gs,Fs]=c2d(A,F,T/p);

% Matriz de Normalização  
Tn=[1/Wbase    0
       0    1/Ibase];
   
% Matrizes de la Planta Normalizadas
An=Tn*A*inv(Tn);
Bn=Tn*B*Vbase;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Discretizacion de la planta para proyecto del controlador por realimentación de estados
[Gn,Hn]=c2d(An,Bn,Tm);
% Esta es discretizacion que usaron el grupo de Schuster y Cia.
% Gn=eye(2)+An*Tm;
% Hn=Bn*Tm;

% Determinación de la controlabilidad del sistema
rango_ps=rank(ctrb(Gn,Hn));     % rango debe ser igual al orden del sistema: planta + servo

% Especificaciones de proyecto
Mp=2/100;               % Sobrepaso maximo del 10%. Mp=exp(-pi.sigma/wd)
tp=3;
wd=pi/tp;
sigma=-(log(Mp)*wd/pi);

s1=-sigma+1i*wd;        % Polos dominantes de lazo cerrado
s2=-sigma-1i*wd;        % complejos conjugados en el dominio continuo
snd=-20*sigma;          % Polo no dominante en el dominio continuo

z1=exp(s1*T);           % Polos dominantes de lazo cerrado
z2=exp(s2*T);           % complejos conjugados en el dominio discreto
znd=exp(snd*T);         % Polo no dominante en el dominio discreto

% Definición de las matrices de la planta con el servo
% G(circunfleja), H(circunfleja)
Gc=[    Gn       Hn;
     zeros(1,2)  0];
Hc=[0;0;1];
Cc=[C 0]; Dc=0;

% Proyecto usando ubicación de polos
polos=[z1 z2 znd];
Kplace=place(Gc,Hc,polos);

Kssp=(Kplace+[zeros(1,2) 1])*inv([Gn-eye(2)  Hn;C*Gn  C*Hn]);
K2=Kssp(1:2); % Ganancias para realimentacion de estados
K1=Kssp(3);   % Ganancia del servo

% K2=[-3.8652 -13.8767e-3]; K1=54.7761e-3;
% K2=[3.5780 0.0087]; K1=-1.3875;
% Determinación de la controlabilidad del sistema
rango_ps=rank(ctrb(Gc,Hc));     % rango debe ser igual al orden del sistema: planta + servo

% Verifica si los autovalores se corresponden con los deseados
autovalores=eig(Gc-Hc*Kssp);

% INICIALIZACION DE LAS VARIABLES UTILIZADAS EN LA SIMULACION 
kc=1:np;    % Vector con total de puntos de simulación de las variables en tiempo continuo
            % con periodo de muestreo T/p
ka=1:na;    % Vector con total de puntos de simulación de las variables en tiempo discreto
            % con periodo de muestreo T
            
% Variables de la planta discreta
xd=zeros(2,length(ka));
xm=zeros(2,length(ka));
wa=zeros(1,length(ka));
ia=zeros(1,length(ka));
y=zeros(1,length(ka));
r=zeros(1,length(ka));
e=zeros(1,length(ka));
v=zeros(1,length(ka));
t=zeros(1,length(ka));
u=zeros(1,length(ka));
uc=zeros(1,length(ka));
Tc=zeros(1,length(ka));
Torque=zeros(1,length(ka));
wr=zeros(1,length(ka));
ar=zeros(1,length(ka));

Wstep=Wref;  % Escalon de referencia
Kref=0.5;
Flag_var_ref=0;  % 1=Es para variar referencia y verificar desempeño
                 % 0=Es para variación de torque con referencia constante

for k=2:na;
    t(k)=k*T;
    
    r(k)=Wref/Wbase;
    if Flag_var_ref == 1
        r(k)=Kref*Wref/Wbase;
    end
    
    xm(:,k)=[xd(1,k)/Wbase;xd(2,k)/Ibase];
    wa(k)=xm(1,k);
    ia(k)=xm(2,k);
    y(k)=C*xm(:,k);
    
    if k > 0.5*na && Flag_var_ref == 1 
        r(k)=Wstep/Wbase;
    end
    
    e(k)=r(k)-y(k);
    v(k)=v(k-1) + e(k);
    u(k)=-K2*xm(:,k) + K1*v(k);
    uc(k)=Vbase*u(k);
    
    Tc(k)=0;
    if k > 0.5*na && Flag_var_ref == 0 
        Tc(k)=20;
    end
        
    xd(:,k+1)=Gs*xd(:,k) + Hs*uc(k) + Fs*Tc(k);
    ar(k)=(xd(1,k+1) - xd(1,k))/Tm; 
    wr(k)=xd(1,k);
    Torque(k)=Tc(k) + J*ar(k) + b*wr(k);
%     Torque(k)=Kt*xd(2,k);
end

figure; plot(t,y,'r',t,r,'k'); grid; legend('velocidad angular'); %axis([0 t(length(t)) 0 1700])
figure; plot(t,e,'r'); grid; legend('error de velocidad')
figure; plot(t,v,'k'); grid; legend('servo')
figure; plot(t,u,'m'); grid; legend('accion de control')
figure; plot(t,Torque); grid; legend('torque en el eje')
figure; plot(t,xd(2,1:k)); grid; legend('corriente de armadura')
figure; plot(t,xd(1,1:k)*(60/2/pi)); grid; legend('velocidad en rpm')