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

function dan4a(Xo,Xf,tf,proc);

%Xo=[0.2 0.2];
%Xf=[0.4 0.4];
%tf=1;
%proc=20;
%primer poziva: dan4a([0.2 0.2],[1 1],3,20)

X1=Xo(1); Y1=Xo(2); 
X2=Xf(1); Y2=Xf(2);
a=(Y2-Y1)/(X2-X1);
b=Y1-a*X1;

tc=proc/100*tf;
dt=0.025;
t=[];

tpom=[0:dt:tc];
V=abs(X2-X1)*sqrt(1+a*a)/(tf-tc);
wt=V/tc;

jj=0;
for ii=1:numcols(tpom),
  jj=jj+1;
  t(jj)=(jj-1)*dt;
  X(jj)=X1+0.5*wt*tpom(ii)^2/sqrt(1+a*a)*sign(X2-X1);
  Y(jj)=a*X(jj)+b;
  Xd(jj)=wt*tpom(ii)/sqrt(1+a*a)*sign(X2-X1);
  Yd(jj)=a*Xd(jj);
  Xdd(jj)=wt/sqrt(1+a*a)*sign(X2-X1);
  Ydd(jj)=a*Xdd(jj);
end
Xtc=X(jj);

if proc<50,
  tpom=[tc+dt:dt:tf-tc];
  for ii=1:numcols(tpom),
    jj=jj+1;
    t(jj)=(jj-1)*dt;
    X(jj)=Xtc+V*(tpom(ii)-tc)/sqrt(1+a*a)*sign(X2-X1);
    Y(jj)=a*X(jj)+b;
    Xd(jj)=V/sqrt(1+a*a)*sign(X2-X1);
    Yd(jj)=a*Xd(jj);
    Xdd(jj)=0;
    Ydd(jj)=a*Xdd(jj);
  end
  Xtftc=X(jj);
elseif proc==50,
  Xtftc=Xtc;
else
  disp('proc sme najvise da iznosi 50 %');
end

tpom=[tf-tc+dt:dt:tf];
for ii=1:numcols(tpom),
  jj=jj+1;
  t(jj)=(jj-1)*dt;
  X(jj)=Xtftc+(V*(tpom(ii)-tf+tc)-wt*(tpom(ii)-tf+tc)^2/2)/sqrt(1+a*a)*sign(X2-X1);
  Y(jj)=a*X(jj)+b;
  Xd(jj)=(V-wt*(tpom(ii)-tf+tc))/sqrt(1+a*a)*sign(X2-X1);
  Yd(jj)=a*Xd(jj);
  Xdd(jj)=-wt/sqrt(1+a*a)*sign(X2-X1);
  Ydd(jj)=a*Xdd(jj);
end

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

kindin;

