% 5a. dan racunskih vezbi iz Tehnicke robotike u zimskom semestru skolske 1998/99
% 7. 12. 1998.

%*************
% Zadatak 1. *
%*************

t=[0:0.01:1];
dime=size(t);
q=pi/2*ones(dime)+3*pi/2*t.*t-pi*t.^3;
qt=3*pi*t-3*pi*t.^2;
qtt=3*pi*ones(dime)-6*pi*t;

[q1,qd,qdd]=jtraj(pi/2,pi,t,0,0);

figure(1);
plot(t,q, t,q1,':');

figure(2);
plot(t,qt, t,qd,':');

figure(3);
plot(t,qtt, t,qdd,':');

disp('Pritisni razmaknicu radi nastavka!');
pause;

close all;
clear all;

%*************
% Zadatak 2. *
%*************

puma560;
t0=[0:0.01:0.5];
t1=[0.5:0.01:1];
t=[t0,t1];

qf=[pi/3,pi/4,-pi/4,0,0,0];

[q,qd,qdd]=jtraj(qz,qr,t0);
[qpom,qdpom,qddpom]=jtraj(qr,qf,t0);

q=[q;qpom];
qd=[qd;qdpom];
qdd=[qdd;qddpom];

figure(1);
plot(t,q*180/pi);
legend('q1','q2','q3','q4','q5','q6');

figure(2);
plot(t,qd);
legend('qd1','qd2','qd3','qd4','qd5','qd6');

figure(3);
plot(t,qdd);
legend('qdd1','qdd2','qdd3','qdd4','qdd5','qdd6');

disp('Pritisni razmaknicu radi animacije pokreta !');
pause;

FK=fkine(p560,q);

figure(4);
plot3(FK(:,13),FK(:,14),FK(:,15),'c');
hold;
plotpro2(p560,q);

