% 開リンク機構 (2DOF) 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;
g = 9.8;

%theta1d = (pi)/6; Kp1 = 100; Kd1 = 1.0;
%theta2d = (pi)/4; Kp2 = 100; Kd2 = 1.0;

theta1d = (pi)/6; Kp1 = 100; Kd1 = 10.0; Ki1 = 100;
theta2d = (pi)/4; Kp2 = 100; Kd2 = 10.0; Ki2 = 100;

open_link_PID = @(t,q) open_link_PID_param ...
(t,q, l1,lc1,m1,J1, l2,lc2,m2,J2, g, theta1d, theta2d, Kp1, Ki1, Kd1, Kp2, Ki2, Kd2);

interval = [0,10];
qinit = [0; 0; 0; 0; 0; 0];
[time,q] = ode23(open_link_PID, interval, qinit);

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

ptext = strcat(num2str(Kp1), '-', num2str(Ki1), '-', num2str(Kd1), ...
          '-', num2str(Kp2), '-', num2str(Ki2), '-', num2str(Kd2));

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

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