%% Ejemplos discretización y simulación INTERMUESTREO.
%  (c) 2023 Antonio Sala Piqueras,
%      Catedrático, Universitat Politecnica de Valencia
% Todos los derechos reservados

% Vídeo-presentación en: https://youtu.be/L9UTgnJME7k



%modelamos masa-muelle
% dposicion_dt=velocidad;
% dvelocidad_dt=1/M*(-k*posicion-f*velocidad+FuerzasExternas)
M=0.25; k=5; f=0.9;
Acontinua=[0 1;-k/M -f/M];
Bcontinua=[0;1/M];
%dotx=Ax+Bu






%% respuestea LIBRE: solución ecuación diferencial dxdt=A*x
% expm(A*t)*x_inicial...
 
Tiempofinal=20;
PasoSimulacionContinua=0.025;
rangotiempos=0:PasoSimulacionContinua:Tiempofinal;
NpuntosSimulacion=length(rangotiempos);
x=zeros(2,NpuntosSimulacion);
x_inicial=[4.5;-6];
for i=1:NpuntosSimulacion
    t=rangotiempos(i);
    x(:,i)=expm(Acontinua*t)*x_inicial;% solucion EXACTA EDO lineal.
end
plot(rangotiempos,x,'LineWidth',2)
grid on
xlabel('Tiempo (s)');
legend('Posicion (m)','Velocidad (m/s)');
title('Masa-Muelle: respuesta libre, c.i. fuera del equilibrio.');





%% Discreticemos con retenedor orden cero:
Tmuestreo=0.4;
Aampliada=[Acontinua Bcontinua;zeros(1,3)]

%% discretización
MatrizGorda=expm(Aampliada*Tmuestreo)

Adiscreta=MatrizGorda(1:2,1:2)
Bdiscreta=MatrizGorda(1:2,3)





%% simulacion discreta
% fuerza es 10*sin(0.35t) discretizada
FzaEntrada=@(t) sin(0.75*t)*10;
Nmuestras=ceil(Tiempofinal/Tmuestreo);
rangodiscreto=0:1:Nmuestras;

u=FzaEntrada(rangodiscreto*Tmuestreo); 

xdiscreto=zeros(2,Nmuestras);
xdiscreto(:,1)=x_inicial;
for j=2:(Nmuestras+1)
    xdiscreto(:,j)=Adiscreta*xdiscreto(:,j-1)+Bdiscreta*u(:,j-1);
end
plot(rangodiscreto*Tmuestreo,xdiscreto(1,:),'o');
hold on
plot(rangodiscreto*Tmuestreo,xdiscreto(2,:),'o');
plot(rangodiscreto*Tmuestreo,u,'xk');

hold off
grid on
axis tight
legend('Muestras Posicion','Muestras Velocidad','Muestras Entrada')
xlabel('tiempo (s)');
title('Simulacion masa-muelle discretizado');








%% simulación continua

x=zeros(2,NpuntosSimulacion);
sim_u=zeros(1,NpuntosSimulacion);


NumeroDeMuestra=1;%la primera, Matlab empieza por 1 las matrices.
TiempoUltimaMuestra=0;
EstadoUltimaMuestra=x_inicial;
EntradaUltimaMuestra=u(NumeroDeMuestra);
x(:,1)=x_inicial;

for i=2:NpuntosSimulacion
    t=rangotiempos(i);
    tiempodesdeultimamuestra=t-TiempoUltimaMuestra;
    xi0=[EstadoUltimaMuestra;EntradaUltimaMuestra];
    xi_t=expm(Aampliada*tiempodesdeultimamuestra)*xi0;
    x(:,i)=xi_t(1:2);%sólo el estado se almacena.
    sim_u(:,i)=EntradaUltimaMuestra;
     if((tiempodesdeultimamuestra>=Tmuestreo-1e-7) && (t<Tiempofinal))%es un instante de muestreo!
        % se realiza un muestreo, ya ha pasado el tiempo.
         NumeroDeMuestra=NumeroDeMuestra+1;
        TiempoUltimaMuestra=t;
        EstadoUltimaMuestra=x(:,i);
        EntradaUltimaMuestra=u(NumeroDeMuestra);
    end
end

hold on 
plot(rangotiempos,x(1,:),'Linewidth',4)
plot(rangotiempos,x(2,:),'Linewidth',2)
plot(rangotiempos,[sim_u; FzaEntrada(rangotiempos)])
hold off
legend('Muestras Posicion','Muestras Velocidad','Muestras Entrada','posicion (m)','velocidad (m/s)','Fuerza (ZOH, N)','Fuerza (ideal)','Location','best')
axis tight
grid on
xlabel('Tiempo (s)')
title('Simulación Masa-Muelle muestreado (c) Antonio Sala, UPV');
