% 4b. dan racunskih vezbi iz Tehnicke robotike u zimskom semestru skolske 1998/99
% 5. 12. 1998.
%
% FUNCTION DINSCAL2(Xo,Xf,tf,proc) Vezba za dinamicko skaliranje trajektorija
%                                  po sinusoidalnom profilu intenziteta brzine
%                                  vrha robota

function dan4b(A,B,tf);

%A=[0.2 0.2];
%B=[1 1];
%tf=3;
%primer poziva: dan4b([0.2 0.2],[1 1],3)

dt=0.025;
t=[0:dt:tf];

Xo=A(1);
Xf=B(1);
Yo=A(2);
Yf=B(2);

k=(Yf-Yo)/(Xf-Xo);
n=Yf-k*Xf;

w=2*pi/tf;
a=sqrt(1+k*k)*(Xf-Xo)/tf;
X=Xo*ones(size(t))+(a/(sqrt(1+k*k))*(t-sin(w*t)/w));
Xd=(a/sqrt(1+k*k))*(ones(size(t))-cos(w*t));
Xdd=(a/sqrt(1+k*k))*w*sin(w*t);

Y=k*X+n*ones(size(t));
Yd=k*Xd;
Ydd=k*Xdd;

v=sqrt(Xd.*Xd+Yd.*Yd);
ac=sqrt(Xdd.*Xdd+Ydd.*Ydd);

kindin;

