% ホロノミック制約の安定化
% 単振り子（デカルト座標系）の運動方程式を数値的に解く

global len;
global mass;
global grav;
global nu;

len = 2.00;
mass = 0.01;
grav = 9.8;
%len = grav*(5/(2*pi))^2; % 周期 5s

nu = 2000;

%interval=0:0.1:10; % 固定ステップ
interval = [0,10]; % 可変ステップ
thetainit = pi/3; % 初期角度
qinit = [len*sin(thetainit); len*(1-cos(thetainit)); 0; 0];
[time, q] = ode45(@pendulum_Cartesian, interval, qinit);

set(0,'defaultAxesFontSize',16);
set(0,'defaultTextFontSize',16);

plot(time,q(:,1),time,q(:,2),'--');
%title('position x and y');
xlabel('time');
ylabel('position');
legend('x', 'y');
grid on;
saveas(gcf, 'pendulum-xy-position.eps', 'eps');
fprintf("t-x t-y グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

plot(time,q(:,3),time,q(:,4),'--');
%title('velocity v_x and v_y');
xlabel('time');
ylabel('velocity');
legend('v_x', 'v_y');
grid on;
saveas(gcf, 'pendulum-xy-velocity.eps', 'eps');
fprintf("t-vx t-vy グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

theta = atan2(q(:,1),len-q(:,2));
plot(time,theta);
%title('angle \theta');
xlabel('time');
ylabel('angle');
legend('\theta');
grid on;
saveas(gcf, 'pendulum-xy-computed-angle.eps', 'eps');
fprintf("t-theta グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

constraint = sqrt(power(q(:,1),2)+power(q(:,2)-len,2))-len;
plot(time,constraint);
ylim([-0.001,0.001]);
%title('constraint R');
xlabel('time');
ylabel('constraint');
legend('R');
grid on;
saveas(gcf, 'pendulum-xy-computed-constraint.eps', 'eps');
fprintf("t-制約 グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

plot(q(:,1),q(:,2));
ylim([-0.2,1.2]);
xlabel('x');
ylabel('y');
grid on;
saveas(gcf, 'pendulum-xy-path.eps', 'eps');
fprintf("単振り子の軌跡\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;
