%% Simulación de péndulo-carrito (ode45)

% debe ejecutarse CartPoleWithRotInertia.mlx antes de ejecutarse este
% archivo.
if ~exist('a_xEL')
    run('CartPoleWithRotInertia.mlx')
end
%Vídeo: http://personales.upv.es/asala/YT/V/cpinesim.html

ParamsConst= {J, L, M, b_th, b_x, g, m};
ValoresParamsConst= {.25, 1, .4, 0.2, 0.14, 9.81, 0.85};
aceleraciones=subs([a_xEL; a_thetaEL],ParamsConst,ValoresParamsConst);
%compilamos de symbolic Toolbox a "código Matlab que manipula números":
d2qdt=matlabFunction(aceleraciones,'Vars',{[x; theta; v_x; v_theta], F});
%vector de estado es [x; theta; v_x; v_theta] para integración numérica

F_ext_parasimular=@(t) 0*sqrt(t+0.5)*sin(1.8*t)^3;
dEstadodt=@(t,estado) [estado(3);
                       estado(4);
                       d2qdt(estado, F_ext_parasimular(t))];

EstadoInicial=[1.9;0.03;0;0];

opts=odeset('AbsTol',1e-6,'RelTol',1e-5); %probar 8e-5 sin fricción.
Tfinal=25;
[TiemposEL,EstadosEL]=ode45(dEstadodt,[0 Tfinal],EstadoInicial,opts);

%gráfica respuesta temporal de los dos primeros estados (posiciones)
figure(1)
plot(TiemposEL,EstadosEL(:,1:2),'LineWidth',4), grid on, xlabel('Tiempo'), ylabel('Posición'), legend("x", "\theta")

%return

%animación del carro-péndulo
figure(2)
Tfin=TiemposEL(end);

PosicionX=EstadosEL(:,1);

%a partir de x y theta obtenemos la posición del cdg de la bola...
%lo compilaremos a numérico
PosicionCdgBolaNum= ...
    matlabFunction(subs(pos_cdg_bola,ParamsConst,ValoresParamsConst),'Vars',{[x;theta]});


%interpolamos la simulación de x y theta en intervalos regulares
dT_anim=0.06; %período de tiempo simulado entre frames
T_anim=0:dT_anim:Tfin; Nmuestras=length(T_anim);
PosicionesAnim=interp1(TiemposEL,EstadosEL(:,1:2),T_anim);
PosicionBolaAnim=PosicionCdgBolaNum(PosicionesAnim'); %posición CDG de la bola

mx=min(PosicionX);Mx=max(PosicionX); %para los ejes de la gráfica

for k=1:Nmuestras 
    tic
    plot(PosicionBolaAnim(1,k),PosicionBolaAnim(2,k),'or','MarkerSize',16,'LineWidth',6), hold on %bola CdG
    plot(PosicionBolaAnim(1,k)*0.85+PosicionesAnim(k)*0.15,0.85*PosicionBolaAnim(2,k)+0.15*0.03,'or','MarkerSize',24,'LineWidth',5) %bola cerca
    plot(PosicionBolaAnim(1,k)*1.07-PosicionesAnim(k)*0.07,1.07*PosicionBolaAnim(2,k)-0.07*0.03,'or','MarkerSize',32,'LineWidth',8) %bola lejos
    rectangle('Position',[PosicionesAnim(k)-0.4,0.003,0.8,0.1],'LineWidth',6,'Curvature',0.75) %carro
    plot(PosicionesAnim(k),0.03,'ob','LineWidth',3), %eje rotación bolas colgadas
    line([PosicionesAnim(k) PosicionBolaAnim(1,k)],[0.03 PosicionBolaAnim(2,k)],'Color',[.75 0 .4],'LineWidth',6) %barra
    yline(-0.01,'b','LineWidth',3) %carril para carro
    axis equal
    grid on
    axis([mx-1.25 Mx+1.25 -1.25 1.25])
    hold off
    drawnow
    toc
    pause(1/20-0.12) %bueno, Matlab no es de tiempo real, esto es "muy aproximado"
end

