% PD control of 2DOF open link mechanism

param = open_link_2DOF_definition ();
gravity = 9.8;
thetad = [ pi/3; pi/6 ];
Kp = [ 3; 3 ];
Kd = [ 0.5; 0.5 ];
open_link_2DOF_PD_ode = @(t,q) open_link_2DOF_PD_params (t, q, param, gravity, thetad, Kp, Kd);

interval = [ 0, 10 ];
thetainit = [ 0; 0 ]; omegainit = [ 0; 0 ];
qinit = [thetainit; omegainit];
[time, q] = ode45(open_link_2DOF_PD_ode, interval, qinit);

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

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

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