%% Caso de estudio motor eléctrico: modelado, modelado de orden reducido, respuesta temporal Laplace ante escalón, y ante tren de escalones
% © 2022, Antonio Sala Piqueras, Universitat Politècnica de València
% Presentaciones en vídeo:
% http://personales.upv.es/asala/YT/V/motccmodel.html ,
% http://personales.upv.es/asala/YT/V/motcctfstep.html ,
% http://personales.upv.es/asala/YT/V/motccLapl.html .
% Este código funcionó correctamente en Matlab R2022b (Linux)
% Objetivos: modelar un motor eléctrico de CC, haciendo girar una carga de cierta inercia. Obtener su respuesta temporal ante escalón y ante un tren de pulsos, usando método de Transformada de Laplace.
% Tabla de Contenidos
% Modelado
%         Modelo completo (con dinámica parte eléctrica)
%         Modelo simplificado (sin dinámica en parte eléctrica, L=0)
%     Matriz de transferencia con symbolic toolbox
%     Introducción modelo en Control Systems Toolbox y función (matriz) de transferencia
% Respuesta temporal ante escalón
%     Representación gráfica con Control Systems Toolbox
%     Cálculos teóricos (Laplace, Symbolic Toolbox)
%         Salida (en dominio de Laplace)
% Respuesta temporal ante tren de escalones
%
%% Modelado
%
%% Modelo completo (con dinámica parte eléctrica)
% Teniendo en cuenta que en un motor eléctrico de corriente continua, en unidades internacionales la tensión generada es E=k\cdot \omega, y el par motor mecánico es T=k\cdot i, las ecuaciones de la física serán:
% \frac{d\theta}{dt}=\omega
% \frac{d\omega}{dt}=\frac{1}{J}T_{res}=\frac{-b}{J}\omega +\frac{k}{J}i
% \frac{di}{dt}=\frac{1}{L}V_L=\frac{1}{L}(V-iR-k\omega)
% Representación interna matricial normalizada (se trata de un sistema lineal) \dot x=Ax+Bu, y=Cx+Du:

syms k R L b J real
A=[0 1 0;0 -b/J k/J;0 -k/L -R/L];
B=[0;0;1/L];
C=[1 0 0;0 1 0];
D=[0;0];

% donde hemos tomado posición y velocidad angular como salidas de interés para construir C y D.
%
%% Modelo simplificado (sin dinámica en parte eléctrica, L=0)
% Entonces ya el sistema es de segundo orden, no de tercero... no podemos "dividir por L" (que sería "infinito") en la tercera ecuación, por lo que escribimos L\frac{di}{dt}=V_L... o sea V_L=0=V-iR-k\omega.
% Las ecuaciones quedan:

syms theta w i V real
syms dthetadt dwdt real
ModeloSimplificado=  [dthetadt;dwdt;0] == [w; -b/J*w+k/J*i ; -k*w-R*i+V] 

% pero este modelo no está en representación interna: hay que eliminar la variable i usando la tercera ecuación, y sustituirla en la segunda... se lo dejamos a Matlab:

solsimple=solve(ModeloSimplificado,{dthetadt;dwdt;i})
EcEstadoSimple=[solsimple.dthetadt;solsimple.dwdt]
Asimple=jacobian(EcEstadoSimple,[theta,w])
Bsimple=jacobian(EcEstadoSimple,V)
Csimple=[1 0;0 1]; Dsimple=[0;0]; %queremos posición y velocidad, o sea TODO el estado.

%
%% Matriz de transferencia con symbolic toolbox

syms s
Estado=[theta;w;i]; %del modelo 3er orden
ModeloLaplace= s*Estado==A*Estado+B*V
sol=solve(ModeloLaplace,{theta,w,i})

% Lo que multiplica a V en cada vble de estado sería la matriz de transferencia de entrada a estados:

collect(diff([sol.theta;sol.w;sol.i],V),s)

% Las FdT de posición y velocidad angular (salidas de interés) serían las dos primeras.
% Con la fórmula "matricial" teórica sale, obviamente, lo mismo:

MdT=collect(C*inv(s*eye(3)-A)*B+D,s) %no nos interesa i(s) en la matriz de transferencia final

% En el modelo "simplificado" sin autoinduccion de armadura:

MdT_simple=Csimple*inv(s*eye(2)-Asimple)*Bsimple+Dsimple

% Coincide con hacer L=0 en la MdT, como era de esperar... aunque, como hemos visto, no se podia hacer L=0 en la representacion interna normalizada. En la MdT como no salia L "dividiendo" a nada, habriamos podido llegar al modelo simple de forma facil, al menos en este ejemplo.
%
% Sustituyendo parámetros constantes por sus valores:

R=1.11; L=0.01; k=0.7; b=0.01; J=0.05; %parámetros constantes

% Evaluemos en qué quedan las expresiones simbólicas:

MdT=simplify(eval(MdT))
MdT_simple=simplify(eval(MdT_simple))

%% Introducción modelo en Control Systems Toolbox y función (matriz) de transferencia
% Trabajamos ahora en numérico

A=eval(A) 
B=eval(B)
sys=ss(A,B,C,D);
sys.InputName='V'; sys.OutputName={'\theta','\omega'};
tf(sys)
Asimple=eval(Asimple);Bsimple=eval(Bsimple);
sys_simple=ss(Asimple,Bsimple,Csimple,Dsimple);sys_simple.InputName='V';sys_simple.OutputName={'\theta','\omega'};
tf(sys_simple)

%% Respuesta temporal ante escalón
%% Representación gráfica con Control Systems Toolbox

step(sys,sys_simple,1.5), grid on %sólo 1.5 segundos.
title("Respuesta Escalón (Control Systems Toolbox)")

%% Cálculos teóricos (Laplace, Symbolic Toolbox)
% Las exponenciales de la respuesta libre son los valores propios de A:

eig(A)' %modelo orden 3
eig(Asimple)' %modelo sin dinámica eléctrica.

% Coinciden con las raíces del denominador de las funciones de transferencia; por ejemplo, el modelo "completo"

[Num,Den]=numden(MdT)
vpa(solve(Den(1)==0),6) %en simbólico
vpa(solve(Den(2)==0),6) %en numérico

% Estas exponenciales estarán presentes (salvo casos especialmente preparados) también en la respuesta forzada.
%% Salida (en dominio de Laplace)

Y_s=MdT*1/s
partfrac(Y_s) %no me vale, porque intenta factorizar en el dominio de los números racionales.
pfy=vpa(partfrac(Y_s,FactorMode="real"), 6) %ahora sí sale lo que buscamos
resp_escalon_sym=vpa(ilaplace(pfy),6)
fplot(resp_escalon_sym,[0 2],LineWidth=2), grid on, legend("Posición ang.","Velocidad ang.",Location="best"), title("Salida Symbolic Toolbox")

% El modelo "simplificado" tendrá respuesta ante escalón dada por:

Y_s2=MdT_simple*1/s
PF2=vpa(partfrac(Y_s2,FactorMode="real"),6)
Y_t_simple=vpa(ilaplace(PF2),6)

%%
%% Respuesta temporal ante tren de escalones

u_s=1/s*(1-1.5*exp(-s)+0.75*exp(-2*s)); %tren de pulsos
fplot(ilaplace(u_s),[0 5], LineWidth=2, Color=[0 0.5 0.4]), grid on, ylim([-1 1.5])
title("Tren de escalones de entrada (u(t)=0 \forall t<0)")

% Como hemos calculado la respuesta ante escalón pero "u(t)" es una superposición de escalones retrasados, no necesitamos hacer nada más en Transformada de Laplace:
% El modelo completo con L\neq 0 da como resultado, si llamamos \phi(t) a la respuesta ante escalón unitario (ya calculada):
% y(t)=\phi(t)\ +\ \begin{cases}0 &0\leq t<1 \\ -1.5\phi(t-1) &1\leq t\end{cases}\ +\ \begin{cases}0 &0\leq t<2 \\ 0.75\phi(t-2) &t\geq 2\end{cases}
% Equivalentemente, podemos expresarlo como:
% y(t)\ =\ \begin{cases}\phi(t) &0\leq t<1 \\ \phi(t)-1.5\phi(t-1) &1\leq t<2 \\ \phi(t)-1.5\phi(t-1)+0.75\phi(t-2) &t\geq 2\end{cases}
%
% *Para graficarlo, recordemos que las funciones "a trozos" necesitan ser introducidas con el comando "heaviside" de la symbolic toolbox:

syms t real
resp_pulsos_sym= resp_escalon_sym ...
    - 1.5*heaviside(t-1)*subs(resp_escalon_sym, t, t-1) ...
    + 0.75*heaviside(t-2)*subs(resp_escalon_sym, t, t-2);
fplot(resp_pulsos_sym,[0 5], LineWidth=2), grid on,
legend("Posición ang.","Velocidad ang.",Location="best"),
title("Salida ante tren de escalones")

%