% 閉リンク機構 PID制御

m1 = 1.0; l1 = 0.4; lc1 = 0.2; J1 = 0.01;
m2 = 1.0; l2 = 0.4; lc2 = 0.2; J2 = 0.01;
m3 = 1.0; l3 = 0.4; lc3 = 0.2; J3 = 0.01;
m4 = 1.0; l4 = 0.4; lc4 = 0.2; J4 = 0.01;
g = 9.8;

x1 = -0.2; y1 = 0; x3 = 0.2; y3 = 0; alpha = 10000;

%theta1d = (pi)/6; Kp1 = 100; Kd1 = 10.0; Ki1 = 0.1;
%theta3d = (pi)/4; Kp3 = 100; Kd3 = 10.0; Ki3 = 0.1;

theta1d = pi/2-pi/6; Kp1 = 100; Kd1 = 10.0; Ki1 = 10.0;
theta3d = pi/2-pi/3; Kp3 = 100; Kd3 = 10.0; Ki3 = 10.0;

closed_link_PID = @(t,q) closed_link_PID_param ...
(t,q, l1,lc1,m1,J1, l2,lc2,m2,J2, l3,lc3,m3,J3, l4,lc4,m4,J4, g, ...
 x1,y1, x3,y3, alpha, ...
 theta1d, theta3d, Kp1, Ki1, Kd1, Kp3, Ki3, Kd3);

interval = [0,10];
qinit = [ pi/2; -pi/6; pi/2; pi/6; 0;0;0;0; 0;0 ];
%[time,q] = ode45(closed_link_PID, interval, qinit);
[time,q] = ode23tb(closed_link_PID, interval, qinit);

clf('reset');

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

ptext = strcat(num2str(Kp1), '-', num2str(Ki1), '-', num2str(Kd1), ...
          '-', num2str(Kp3), '-', num2str(Ki3), '-', num2str(Kd3));

plot(time, q(:,1), time, theta1d*ones(size(time)), '--');
ylim([0,2]);
xlabel('time');
ylabel('angle');
legend('\theta_{1}', '\theta_{1}^{d}');
grid on;
filename = strcat('closed-link/PID-', ptext, '-theta1.eps');
saveas(gcf, filename, 'eps');
fprintf("t-theta1 グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

plot(time, q(:,3), time, theta3d*ones(size(time)), '--');
ylim([0,2]);
xlabel('time');
ylabel('angle');
legend('\theta_{3}', '\theta_{3}^{d}');
grid on;
filename = strcat('closed-link/PID-', ptext, '-theta3.eps');
saveas(gcf, filename, 'eps');
fprintf("t-theta3 グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

[ X, Y ] = closed_link_constraints ...
( q(:,1), q(:,2), q(:,3), q(:,4), l1, l2, l3, l4, x1, y1, x3, y3 );

fprintf("制約 X の絶対値の最大値 %.12f\n", max(abs(X)));
fprintf("制約 Y の絶対値の最大値 %.12f\n", max(abs(Y)));

%[X,Y] = closed_link_X_Y( pi/2, -pi/6, pi/2, pi/6, l1, l2, l3, l4, x1, y1, x3, y3);
%closed_link_draw (  pi/2, -pi/6, pi/2, pi/6, l1, l2, l3, l4, x1, y1, x3, y3 );

clf('reset');
for t = [ 0:0.01:0.1, 0.1:0.1:1, 1:1:10]
    fprintf("time : %f\n", t);
    index = nearest_index(time, t);
    theta1 = q(index,1); theta2 = q(index,2); theta3 = q(index,3); theta4 = q(index,4);
    closed_link_draw(theta1, theta2, theta3, theta4, l1, l2, l3, l4, x1, y1, x3, y3 )
    hold off;
    xlim([-0.5,0.7]);
    ylim([-0.1,1.1]);
    set(gca,'DataAspectRatio',[1 1 1]);
    grid on;
    filename = strcat('closed-link/', num2str(t), '.eps');
    saveas(gcf, filename, 'eps');
end
