clear all
close all
clc

%% grafico das juntas de um robô cartesiano
P=[5.000000	-2.000000	9.000000;
2.852476	2.400551	10.425770;
2.443977	7.108458	8.560102;
2.107704	12.195803	8.685630;
1.720847	17.279648	8.806692;
1.327363	22.363383	8.909940];

l=size(P);
f=l(1);

% Laço de for para cálculo da cinemática inversa
for i=1:f
    D1(i)=P(i,1);
    D2(i)=P(i,2);
    D3(i)=P(i,3);
end
hi= 10*[0.257948812689560 0.281285761685533 0.148800346778888 0.112401718859996 0.096287419220529];
t(1)=0;
for i=1:f-1
    t(i+1)=t(i)+hi(i);
end
A1=[t;D1];
A2=[t;D2];
A3=[t;D3];
% figure
% subplot(3,1,1)
% spline3(A1);
% subplot(3,1,2)
% spline3(A2);
% subplot(3,1,3)
% spline3(A3);


figure(2)
subplot('Position',[.19 .75 .67 .23]) %[.19 .66 .665 .23]
hold on
spline3(A1);
hold off
% set(gca,'XTick',0:2:10)
% set(gca,'YTick',0:1:7)
xlabel('Time (s)','FontSize',12)
ylabel('Position (cm)','FontSize',12)
xlim([0 9])
% ylim([0 7])
text(-2,3,'\bf{(a)}','FontSize',16);
box on



subplot('Position',[.19 .42 .67 .23])
hold on
spline3(A2);
hold off
% set(gca,'XTick',0:1:9)
% set(gca,'YTick',0:1:7)
xlabel('Time (s)','FontSize',12)
ylabel('Position (cm)','FontSize',12)
xlim([0 9])
% ylim([0 7])
text(-2,10,'\bf{(b)}','FontSize',16);
box on

subplot('Position',[.19 .09 .67 .23])
hold on
spline3(A3);
hold off
% set(gca,'XTick',0:1:9)
% set(gca,'YTick',0:1:7)
xlabel('Time (s)','FontSize',12)
ylabel('Position (cm)','FontSize',12)
xlim([0 9])
% ylim([0 7])
text(-2,9.5,'\bf{(c)}','FontSize',16);
box on