% plotet98.m Graficki prikaz robota robet98
%
%   plotet98 (dh, q, fig)
%
%      dh - matrica DH parametara robota
%      q  - pozicije zglobova
%     fig - broj prozora za crtanje (opciono);
%
%   Pretpostavka je da robot ima 3 rotaciona i 2 transl. ss
%   
%   (c) 1998. Rasic Milan


function plplanr (dh, q, fig);

[dof,par]= size(dh);
if (par<5), error ('Neodgovarajuca matrica DH parametara!'); end;

[np, n]= size(q);
if (n~=dof) error ('Vektor q nema dovoljno kolona!'); end;

if (nargin<3),
   fig=gcf;
end

% Postavljanje osobina slike
figure (fig);
axis ([-0.6 0.4 -0.2 1.2]);
set (gca, 'AspectRatio', [1,1], 'Box','on',...
     'DrawMode', 'fast');

   % Vektor polozaja zglobova
   x=zeros(dof+1,1);
   y=zeros(dof+1,1);

   T=eye(4);  
   T3=eye(4);  % Matrica HT polozaja treceg zgloba
   T4=eye(4);  % Matrica HT polozaja cetvrtog zgloba

   % Odredjivanje polozaja zglobova 
   for j=1:dof,
      T=T*linktran(dh(j,:),q(1,j));  
      if j==3,
        T3=T;
      end;
      if j==4,
        T4=T;
      end;

      x(j+1) = T(1,4);
      y(j+1) = T(2,4);
  end;

  % Cetvrti zglob se crta ceo a granicni polozaji su +- 0.05
  Tv1= T3*linktran(dh(4,:),-0.05);  
  Tv2= T3*linktran(dh(4,:), 0.05);  
  x4 =[Tv1(1,4), Tv2(1,4)];
  y4 =[Tv1(2,4), Tv2(2,4)];

  % I peti zglob se crta ceo od 0.1-q5 do q5
  Tv3= T4*linktran(dh(5,:),-(0.1-q(1,5)));  
  x5 = [Tv3(1,4), x(6)];  
  y5 = [Tv3(2,4), y(6)];  

  lh1 = line (x(1:4),y(1:4), 'LineWidth',4,...
              'EraseMode','background');              % Crta prva tri zgloba
  lh2 = line (x4,y4, 'LineWidth',2,...
              'EraseMode','background');              % Crta cetvrti zglob
  lh3 = line (x5,y5, 'LineWidth',2,...
              'EraseMode','background');              % Crta peti zglob
  lh4 = line (x(1:3),y(1:3), 'LineStyle',...   % Crta kruzice u prva tri zgloba
        '.', 'Markersize',35,'EraseMode','background',...
         'Clipping','on');

pause;
for i=2:np,
   % Vektor polozaja zglobova
   x=zeros(dof+1,1);
   y=zeros(dof+1,1);

   T=eye(4);  
   T3=eye(4);  % Matrica HT polozaja treceg zgloba
   T4=eye(4);  % Matrica HT polozaja cetvrtog zgloba

   % Odredjivanje polozaja zglobova 
   for j=1:dof,
      T=T*linktran(dh(j,:),q(i,j));  
      if j==3,
        T3=T;
      end;
      if j==4,
        T4=T;
      end;

      x(j+1) = T(1,4);
      y(j+1) = T(2,4);
  end;

  % Cetvrti zglob se crta ceo a granicni polozaji su +- 0.05
  Tv1= T3*linktran(dh(4,:),-0.05);  
  Tv2= T3*linktran(dh(4,:), 0.05);  
  x4 =[Tv1(1,4), Tv2(1,4)];
  y4 =[Tv1(2,4), Tv2(2,4)];

  % I peti zglob se crta ceo od 0.1-q5 do q5
  Tv3= T4*linktran(dh(5,:),-(0.1-q(i,5)));  
  x5 = [Tv3(1,4), x(6)];  
  y5 = [Tv3(2,4), y(6)];  

  set (lh1, 'Xdata',x(1:4),'Ydata',y(1:4));   % Crta prva tri zgloba
  set (lh2, 'Xdata',x4, 'Ydata',y4);          % Crta cetvrti zglob
  set (lh3, 'Xdata',x5, 'Ydata',y5);          % Crta peti zglob
  set (lh4, 'Xdata',x(1:3), 'Ydata',y(1:3));  % Crta kruzice u prva tri zgloba

  drawnow;
end;
