clear
close all

% Definicion de los parametros de simulacion
% fm=100;                 % FRECUENCIA DE MUESTREO EN Hz
% T=1/fm;                 % PERIODO DE MUESTREO
% Tf=40;                  % TIEMPO TOTAL DE SIMULACION
% na=Tf/T;                % NUMERO DE TOTAL DE MUESTRAS EN LA SIMULACION
na=4000;                % NUMERO DE TOTAL DE MUESTRAS EN LA SIMULACION
p=128;                  % 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.
Wmbase=2000*(2*pi/60);   % VALOR BASE DE VELOCIDAD ANGULAR PARA NORMALIZACION (VELOCIDAD NOMINAL) EN RAD/SEG
Wcbase=2000*(2*pi/60);   % VALOR BASE DE VELOCIDAD ANGULAR PARA NORMALIZACION (VELOCIDAD NOMINAL) EN RAD/SEG
Tebase=10;               % VALOR BASE DE TORQUE ELASTICO PARA NORMALIZACION EN 
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
Tbase=10;                % VALOR DEL TORQUE BASE EN Nxm
Wmref=2000*(2*pi/60);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETROS DE LA PLANTA.


% Definición de las matrices de estado del sistema
% Variables de estado: x1=,  x2=,  x3=, x4=


[num_c,den_c]=ss2tf(A,B,C,D);
Gts=tf(num_c,den_c);

sys_c=ss(A,B,C,D);

spec=stepinfo(sys_c);
ts_la=spec.SettlingTime;
figure; step(sys_c)

% Calculo del periodo de muestreo
Nd=;
Td=
T=;

% figure; pzmap(sys_c);

%% Aqui se verifica el desempeño natural del sistema para un conjunto dado de condiciones iniciales 
xi=[0;0;0;0];
tc=0:T:20;
[yy,tc,x]=initial(sys_c,xi,tc);
 
figure; plot(tc,x(:,1)); grid; legend('velocidad angular motor')
figure; plot(tc,x(:,2)); grid; legend('velocidad angular carga')
figure; plot(tc,x(:,3)); grid; legend('torque elastico')
figure; plot(tc,x(:,4)); grid; legend('corriente de armadura')
% Determinación de la controlabilidad del sistema
rango_planta=rank(ctrb(A,B));        % rango debe ser igual al orden del sistema

%% Matriz de Normalización  
Tn=
   
% Matrizes de la Planta Normalizadas
An=Tn*A*inv(Tn);
Bn=Tn*B*Xbase;
Fn=Tn*F*Xbase;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Discretizacion de la planta para proyecto del controlador por
% realimentación de estados utilizando un 'zoh'
[G,H]=c2d(An,Bn,T);
[GG,Fd]=c2d(An,Fn,T);

% Determinación de la controlabilidad del sistema discreto

rango_mc=;     % rango debe ser igual al orden de la planta

% "El sistema es totalmente controlable"

% Especificaciones de proyecto
Mp=;               % Sobrepaso maximo del 1%. Mp=exp(-pi.sigma/wd)
tp=;

s1=-sigma+1i*wd;        % Polos dominantes de lazo cerrado
s2=-sigma-1i*wd;        % complejos conjugados en el dominio continuo
snd1=-x*sigma;          % Polo no dominante en el dominio continuo
snd2=-x*sigma;          % Polo no dominante en el dominio continuo
snd3=-x*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
znd3=exp(snd3*T);       % Polo no dominante en el dominio discreto

% Definición de las matrices de la planta con el servo
% G(circunfleja), H(circunfleja)


% Proyecto usando ubicación de polos
polos=;
Kplace=;

Kssp=;
K2=Kssp(1:4);   % Ganancias para realimentacion de estados
K1=Kssp(5);     % Ganancia del servo

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


%% Aqui se verifica el diseño a través de simulación del desempeño para entrada en escalon
Gs=[     G-H*K2          H*K1
    (C*H*K2 - C*G)  1 - C*H*K1];
Hs=Hc;
sistema_pca=ss(Gs,Hs,Cc,Dc,T);
tca=0:T:6;
figure; step(sistema_pca,tca)

% close all
% pause
%% *******************************************************************************
% Discretizacion de las matrices para simulación de la planta en tiempo continuo
% con periodo de muestreo=T/p. Planta sin normalizar

Tmc=T/p;
[Gs,Hs]=c2d(A,B,Tmc);
[Gss,Fs]=c2d(A,F,Tmc);

%% 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
xm=zeros(4,length(ka));
xd=zeros(4,length(ka));
wm=zeros(1,length(ka));
wc=zeros(1,length(ka));
Te=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));
u=zeros(1,length(ka));
ulim=zeros(1,length(ka));
uc=zeros(1,length(ka));
td=zeros(1,length(ka));
rampa_d=zeros(1,length(ka));

% Variables de la simulación continua
xc=zeros(4,length(kc));
ua=zeros(1,length(kc));
Tc=zeros(1,length(kc));
Tem=zeros(1,length(kc));
wrc=zeros(1,length(kc));
arc=zeros(1,length(kc));
t=zeros(1,length(kc));
Timer=zeros(1,length(kc));
rampa=zeros(1,length(kc));

Wstep=Wmref;    % Escalon de referencia
Kref=0.5;
Flag_var=0;          % 1 = Es para variar referencia y verificar desempeño
                     % 0 = Es para variación de torque con referencia constante
flag=1;
counter=0;
km=1;

% Inicia Barra de Progresión
perc=0.01;
passo=0.01;
wb = waitbar(0,'Simulando Motor CC...');

for k=1:np;
    t(k)=k*T/p;
    rampa(k)=t(k);
    
    if flag==1
        flag=0;
        km=km+1;
        td(km)=km*T;
        xm(:,km)=xc(:,k);
    end
    
    if counter==p
        rampa_d(km) = rampa(k);
        % Perfil de variación de referencia para verificar
        % especificaciones. Arranque en rampa.
        if km < na/(na/500) && Flag_var == 1
            r(km) = (0.5/5)*rampa_d(km);
        else
            r(km)=Kref*Wmref/Wmbase;
        end
        
        if km > 0.5*na && Flag_var == 1
            r(km)=Wmref/Wmbase;
        end
        % Fin perfil de variación de referencia %
        
        % Perfil de referencia para verificar desempeño con carga
        % Arranque en rampa.
        if km < na/(na/500) && Flag_var == 0
            r(km) = (1/5)*rampa_d(km);
        else if Flag_var == 0
            r(km)=Wmref/Wmbase;
            end
        end
        % Fin perfil de referencia para desempeño de carga %
        
%         if Flag_var == 0
%             r(km)=Wmref/Wmbase;
%         end        
        
        % Normalización variables muestreadas
        wm(km)=xm(1,km)/Wmbase;
        wc(km)=xm(2,km)/Wcbase;
        Te(km)=xm(3,km)/Tebase;
        ia(km)=xm(4,km)/Ibase;
        
        % Vector de estado discreto normalizado
        xd(:,km)=[wm(km);wc(km);Te(km);ia(km)];
        
        % Ecuaciones de calculo dentro del procesador
        % Ecuacion del error
        
        % Ecuacion del servo controlador
        
        % Ecuacion de la accion de control
        
        
%         if u(km) > 10
%             ulim(km) = 1;
%             v(km) = (ulim(km) + K2*xd(:,km))/K1;
%         else
%             ulim(km) = u(km);
%         end
    end
    
    if counter==p
        counter=0;
        flag=1;
    end
    
    Tc(k)=0;
    if k > 0.5*np && Flag_var == 0
        Tc(k)=2.5;
    end

    % Acción del actuador en valor real
    ua(k)=u(km-1)*Vbase;
    
    % Simulación de la planta en tiempo continuo
    xc(:,k+1)=Gs*xc(:,k) + Hs*ua(k) + Fs*Tc(k);
    
    % Cálculo de la aceleración angular en la carga
    arc(k)=(xc(2,k+1) - xc(2,k))/Tmc;
    
    % Velocidad angular en la carga
    wrc(k)=xc(2,k);
    
    % Cálculo del par electromagnético total
    
    
    counter=counter+1;
    Timer(k)=counter;
    
    % Actualiza la barra de progresion
    if fix((round(np))*perc)==k
        waitbar(perc)
        perc=perc+passo;
    end;
end
close(wb) % Cierra la barra de progresion

% Variables en tiempo discreto
figure; plot(td(1:km-1),y,'r',td(1:km-1),r,'k'); grid; legend('velocidad angular en la carga');
% axis([0 max(td) 0 1.1])
figure; plot(td(1:km-1),e,'r'); grid; legend('error de posición')
axis([0 max(td) 0 1.0])
figure; plot(td(1:km-1),v,'k'); grid; legend('acción integral')
figure; plot(td(1:km-1),u,'m'); grid; legend('acción de control')
figure; plot(td(1:km-1),ulim); grid; legend('u limitada')

% Variables en tiempo continuo
figure; plot(t,xc(1,1:k)*(60/2/pi)); grid; legend('Velocidad Angular en el Eje del Motor'); ylabel('RPM'); xlabel('Tiempo, s')
figure; plot(t,xc(2,1:k)*(60/2/pi),td(1:km-1),Wcbase*(60/2/pi)*r); grid; legend('Velocidad Angular en la Carga'); ylabel('RPM'); xlabel('Tiempo, s')
figure; plot(t,xc(3,1:k)); grid; legend('Par Elastico'); ylabel('Par Elastico, N x m'); xlabel('Tiempo, s')
figure; plot(t,xc(4,1:k)); grid; legend('Corriente de Armadura'); ylabel('Corriente ia, Amperes'); xlabel('Tiempo, s')

% figure; plot(t,Tem,t,Tc*1); grid; legend('Par Electromagnetico','Par de Carga (x1)'); ylabel('Par, Nxm'); xlabel('Tiempo, s')
figure; plot(t,arc*(60/2/pi),t,wrc*(60/2/pi)); grid; legend('aceleración','velocidad'); ylabel('RPM'); xlabel('Tiempo, s')
% figure; plot(t,xc(2,1:k),td(1:km-1),Wcbase*r); grid; legend('velocidad angular carga'); ylabel('Radianes'); xlabel('Tiempo, s')