% % Position of Link(1)
figure;
subplot(2,1,1)
plot(q1,'LineWidth',2);
hold on
plot(reff1,'r','LineWidth',2);
legend('Output Control','Desired Response')
title('Position of Link(1)');
grid;
% % -----------------------------------------------
% % Position of Link(2)
subplot(2,1,2)
plot(q2,'LineWidth',2);
hold on
plot(reff2,'r','LineWidth',2);
legend('Output Control','Desired Response')
title('Position of Link(2)');
grid;
% % -----------------------------------------------
% % Error of Link(1)
figure
subplot(2,1,1)
er1=error1.signals.values;
plot(er1,'r','LineWidth',2);
title('Error of Link(1)');
grid;
% % -----------------------------------------------
% % Error of Link(2)
subplot(2,1,2)
er2=error2.signals.values;
plot(er2,'r','LineWidth',2);
title('Error of Link(2)');
grid;
% % -----------------------------------------------
plotregression(reff1,q1);
h1=get(gca,'Title');
st1=get(h1,'String');
st1=st1(12:end);
title({'Validation of Link(1)';[st1,' & ' 'R^2 = ' num2str(R2_1),' & ' 'RMSE = ' num2str(sqrt(MSE1))]});
plotregression(reff2,q2);
h2=get(gca,'Title');
st2=get(h2,'String');
st2=st2(12:end);
title({'Validation of Link(2)';[st2,' & ' 'R^2 = ' num2str(R2_2),' & ' 'RMSE = ' num2str(sqrt(MSE2))]});
% % -----------------------------------------------
% Phase Plane
figure;
subplot(1,2,1);
plot(q1,dq1);
title('Phase Plane for Link(1)');
axis equal;
subplot(1,2,2);
plot(q2,dq2);
title('Phase Plane for Link(2)');
axis equal;
% % -----------------------------------------------
% % -----------------------------------------------
figure;
subplot(1,2,1);
plot3(q1,dq1,t);
title('Phase Plane for Link(1)');
grid;
% axis equal;
subplot(1,2,2);
plot3(q2,dq2,t);
title('Phase Plane for Link(2)');
grid;
% axis equal;
% % -----------------------------------------------
figure;
subplot(2,1,1);
plot(t,q1)
title('Link(1)');
xlabel('t'); ylabel('q_1');
grid;
subplot(2,1,2);
plot(t,q2)
title('Link(2)');
xlabel('t'); ylabel('q_2');
grid;
