%Este programa ejecuta la simulación de un sistema no lineal
%utilizando ecuaciones incrementales, con paso de integracion
%fijo (dt), durante final_time unidades de tiempo.

%Constantes del sistema no lineal y entrada:
mc=10;          % 1 kg
b=0.05;         % constante del amotiguador por pistón
k=1;            % constante del amotiguador por resorte
P=10;           % entrada del sistema no lineal
dt=0.005;       % paso de integracion
final_time=70;  % tiempo de simulacion
pp=6.5;         % entrada del sistema lineal

%Inicialización de los vectores acumuladores de datos, SISTEMA NO LINEAL:
Xtot=[0;0]; 
Xb=[0;0];                
Ttot=[0];      % Para crear y luego almacenar el tiempo en un vector  
                             
x1a=2;         % Condiciones                 
x2a=1;         % Iniciales cuando t=0

x1b=0;         % Condiciones iniciales después del calculo
x2b=0;         

%Inicialización de los vectores acumuladores de datos, SISTEMA LINEALIZADO:
Xltot=[0;0];  % Aquí se aplica todo lo anterior, pero para el linealizado
Xlb=[0;0];

x1la=2;
x2la=1;

x1lb=0;
x2lb=0;

%Simulación del sistema durante el intervalo de tiempo
%definido en "final_time":
for time=0:dt:final_time,   
    %SISTEMA NO LINEAL
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
           x1b=(x2a)*dt + x1a; 
           x2b=(-(k/mc)*x1a + (b/mc)*(1-x1a^2)*x2a + (P/mc))*dt + x2a;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %Guardado de los valores de las variables en este paso:
    Xb=[x1b;x2b];
    Xtot=[Xtot,Xb];
    Ttot=[Ttot,time];

    %Actualización del vector de estados para el siguiente paso:
    x1a=x1b;
    x2a=x2b;
    
    
    %SISTEMA LINEALIZADO
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    x1lb=(x2la   +   0)*dt + x1la;      % Cero (0) es el valor en equilibrio de f1(.) del
                                        % sistema no lineal.
    x2lb=((-k/mc)*x1la + (b/mc)*(1-(pp^2/k^2))*x2la + (pp/mc)  +  (P/mc - pp/mc))*dt + x2la;
                                        % (P/mc - pp/mc) es el valor en
                                        % equilibrio de f2(.) del sistema
                                        % no lineal.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if (time>=35)
        P=5;
    end
    
    %Guardado de los valores de las variables en este paso:
    Xlb=[x1lb;x2lb];
    Xltot=[Xltot,Xlb];

    %Actualización del vector de estados para el siguiente paso:
    x1la=x1lb;
    x2la=x2lb;
    

end

%Graficación de las curvas de estado:
subplot(2,1,1);plot(Ttot,Xtot(1,:),':b');hold;plot(Ttot,Xltot(1,:),'b');hold;
subplot(2,1,2);plot(Ttot,Xtot(2,:),':g');hold;plot(Ttot,Xltot(2,:),'g');hold;