% 5b. dan racunskih vezbi iz Tehnicke robotike u zimskom semestru skolske 1998/99
% 7. 12. 1998.
%
% primer poziva dan5b([0.2 0.2],[0.4 0.4],3,20)

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

Ke=100;
Ked=2*sqrt(Ke);

%Xo=[0.2 0.2];
%Xf=[0.4 0.4];
%tf=1;
%proc=20;

X1=Xo(1); Y1=Xo(2); 
X2=Xf(1); Y2=Xf(2);
a=(Y2-Y1)/(X2-X1);
b=Y1-a*X1;
dt=0.005;
t=[0:dt:tf];

N1=35;
l1=0.42;
m1=3.312;
J1=122e-6;

N2=69;
l2=0.28;
m2=0.42;
J2=1.1261e-6;

%           alfa   a     theta    d    sigma  m     rx      ry      rz       Ixx     Iyy            Izz           Ixy  Iyz  Ixz  Jm   G     B   Tc+   Tc-
ee2rob=[     0     l1      0      0      0    m1   -l1/2    0        0       0       m1*l1*l1/12    m1*l1*l1/12   0    0    0    J1   N1    0   0     0
 	     0     l2      0      0      0    m2   -l2/2    0        0       0       m2*l2*l2/12    m2*l2*l2/12   0    0    0    J2   N2    0   0     0];


tc=proc/100*tf;

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;
  Xref(jj)=X1+0.5*wt*tpom(ii)^2/sqrt(1+a*a)*sign(X2-X1);
  Yref(jj)=a*Xref(jj)+b;
  Xdref(jj)=wt*tpom(ii)/sqrt(1+a*a)*sign(X2-X1);
  Ydref(jj)=a*Xdref(jj);
  Xddref(jj)=wt/sqrt(1+a*a)*sign(X2-X1);
  Yddref(jj)=a*Xddref(jj);
end
Xtc=Xref(jj);

tpom=[tc+dt:dt:tf-tc-dt];
for ii=1:numcols(tpom),
  jj=jj+1;
  Xref(jj)=Xtc+V*(tpom(ii)-tc)/sqrt(1+a*a)*sign(X2-X1);
  Yref(jj)=a*Xref(jj)+b;
  Xdref(jj)=V/sqrt(1+a*a)*sign(X2-X1);
  Ydref(jj)=a*Xdref(jj);
  Xddref(jj)=0;
  Yddref(jj)=a*Xddref(jj);
end
Xtftc=Xref(jj);

tpom=[tf-tc:dt:tf];
for ii=1:numcols(tpom),
  jj=jj+1;
  Xref(jj)=Xtftc+(V*(tpom(ii)-tf+tc)-wt*(tpom(ii)-tf+tc)^2/2)/sqrt(1+a*a)*sign(X2-X1);
  Yref(jj)=a*Xref(jj)+b;
  Xdref(jj)=(V-wt*(tpom(ii)-tf+tc))/sqrt(1+a*a)*sign(X2-X1);
  Ydref(jj)=a*Xdref(jj);
  Xddref(jj)=-wt/sqrt(1+a*a)*sign(X2-X1);
  Yddref(jj)=a*Xddref(jj);
end

q =[1.5094   -2.4086];
qd=[0 0];
qdd=[0 0];

for ii=2:numcols(Xref),

	X_REF=[Xref(ii-1); Yref(ii-1)]; 
	Xd_REF=[Xdref(ii-1); Ydref(ii-1)]; 
	Xdd_REF=[Xddref(ii-1); Yddref(ii-1)]; 

	[J Jdot]=jacobdot(ee2rob,q(ii-1,:),qd(ii-1,:));
	J=[J(1:2,:)];
	Jdot=[Jdot(1:2,:)];

	FX=fkine( ee2rob, q(ii-1,:) );
	X_OS=FX(1:2,4);
	Xd_OS=J*qd(ii-1,:)';

	e=X_REF-X_OS;
	ed=Xd_REF-Xd_OS;

	qdd_ii=(inv(J)*(Xdd_REF+Ked*ed+Ke*e-Jdot*qd(ii-1,:)'))';
	qd_ii=qd(ii-1,:)+qdd(ii-1,:)*dt;
	q_ii=q(ii-1,:)+qd(ii-1,:)*dt+0.5*qdd(ii-1,:)*dt*dt;

        qdd=[qdd;qdd_ii];
        qd=[qd;qd_ii];
        q=[q;q_ii];

end

FX=fkine(ee2rob, q);

vel=sqrt(Xdref.^2+Ydref.^2);
acc=sqrt(Xddref.^2+Yddref.^2);

for ii=1:numcols(acc),
  acc(ii)=acc(ii)*sign(Xddref(ii));
end

figure(1);
plot(t,vel,'y', t,acc,'r');
xlabel('t [s]');
legend('v [m/s]', 'a [m/s^2]');

figure(2);
subplot(3,1,1);
plot(t, Xref);
ylabel('Xref');
subplot(3,1,2);
plot(t, Xdref);
ylabel('Xdref');
subplot(3,1,3);
plot(t, Xddref);
ylabel('Xddref');

figure(3);
subplot(3,1,1);
plot(t, Yref);
ylabel('Yref');
subplot(3,1,2);
plot(t, Ydref);
ylabel('Ydref');
subplot(3,1,3);
plot(t, Yddref);
ylabel('Yddref');

figure(4);
subplot(3,1,1);
plot(t, q(:,1));
ylabel('teta1');
subplot(3,1,2);
plot(t, qd(:,1));
ylabel('tetad1');
subplot(3,1,3);
plot(t, qdd(:,1));
ylabel('tetadd1');

figure(5);
subplot(3,1,1);
plot(t, q(:,2));
ylabel('teta2');
subplot(3,1,2);
plot(t, qd(:,2));
ylabel('tetad2');
subplot(3,1,3);
plot(t, qdd(:,2));
ylabel('tetadd2');

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

figure(6);
plot(FX(:,13),FX(:,14),'r');
hold;
plotpro(ee2rob,q);
xlabel('x [m]');
ylabel('y [m]');

