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.
Titabase=120;
Wbase=2000;             % 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
Titaref=120;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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=theta,  x2=w,  x3=ia

A=[0    1      0
   0  -b/J    Kt/J
   0  -Kb/La -Ra/La];
B=[0;0;1/La];
C=[1 0 0]; D=0;
F=[0;-1/J;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/Titabase    0      0
        0      1/Wbase   0
        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);

% 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
snd1=-20*sigma;         % Polo no dominante en el dominio continuo
snd2=-22*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
znd1=exp(snd1*T);       % Polo no dominante en el dominio discreto
znd2=exp(snd2*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,3)  0];
Hc=[0;0;0;1];
Cc=[C 0]; Dc=0;

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

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

% Estos son los valores de Schuster y Cia
% K2=[140 1451.6 0]; K1=1.1277;  % Linear Quadratic Regulator

% Estos son los valores de Smola y Cia
% K2=[13.7222 90.7954 -6.1019e-3]; K1=136.5029e-3;

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


% 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(3,length(ka));
xm=zeros(3,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));
Tm=zeros(1,length(ka));
wr=zeros(1,length(ka));
ar=zeros(1,length(ka));

for k=2:na;
    t(k)=k*T;
    r(k)=Titaref/Titabase;
    xm(:,k)=[xd(1,k)/Titabase;xd(2,k)/Wbase;xd(3,k)/Ibase];
    tita(k)=xm(1,k);
    wa(k)=xm(2,k);
    ia(k)=xm(3,k);
    y(k)=tita(k);
    
%     if k>0.5*na
%         r(k)=120/Titabase;
%     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
        Tc(k)=200;
    end
        
    xd(:,k+1)=Gs*xd(:,k) + Hs*uc(k) + Fs*Tc(k);
    ar(k)=(xd(1,k) - xd(1,k-1))/T; 
    titar(k)=xd(1,k);
    wr(k)=xd(2,k);
    iar(k)=xd(3,k);
    Tm(k)=Tc(k) + J*ar(k) + b*wr(k);
%     Tm(k)=Kt*iar(k);
end

figure; plot(t,y,'r',t,r,'k'); grid; legend('posicion angular'); %axis([0 t(length(t)) 0 1700])
figure; plot(t,e,'r'); grid; legend('error de posicion')
figure; plot(t,u,'m'); grid; legend('accion de control')
figure; plot(t,v,'m'); grid; legend('servo')
figure; plot(t,Tm); grid; legend('torque en el eje')
figure; plot(t,wr); grid; legend('velocidad angular')
figure; plot(t,iar); grid; legend('corriente en la armadura')