% PD control of 2DOF closed link mechanism

param = closed_link_2DOF_definition ();
gravity = 9.8;
thetad = [ pi/3; pi/6 ];
Kp = [ 3; 3 ];
Kd = [ 0.5; 0.5 ];
alpha = 1000;
closed_link_2DOF_PD_ode = @(t,q) closed_link_2DOF_PD_params (t, q, param, gravity, thetad, Kp, Kd, alpha);

interval = [ 0, 10 ];
thetainit = [ pi/2; -pi/6; pi/2; pi/6 ]; omegainit = [ 0; 0; 0; 0 ];
qinit = [thetainit; omegainit];
[time, q] = ode45(closed_link_2DOF_PD_ode, interval, qinit);

thetad1 = thetad(1)*ones(size(time));
thetad3 = thetad(2)*ones(size(time));
plot(time, q(:,1), 'r-', time, q(:,3), 'b-', ...
     time, thetad1, 'r--', time, thetad3, 'b--');
grid on;
saveas(gcf, 'closed_link_2DOF_PD_angles.png');
fprintf("pause: press any key\n");
pause;

theta = q(:,1:4);
sz = size(time);
for k=1:100:sz(1)
    clf;
    closed_link_2DOF_draw(theta(k,:), param);
    xlim([-4,4]);
    ylim([ 0,4]);
    pbaspect([2 1 1]);
    grid on;
    pause(0.01);
end
fprintf("pause: press any key\n");
pause;

clf;
hold on;
for k=1:100:sz(1)
    closed_link_2DOF_draw(theta(k,:), param);
    xlim([-4,4]);
    ylim([ 0,4]);
    pbaspect([2 1 1]);
    grid on;
    pause(0.01);
end
hold off;
saveas(gcf, 'closed_link_2DOF_PD_draw.png');
