% PDCTL     Realizuje PD regulator za upravljanje robotom
% 
% Funkcija koja realizje PID kontroler - racuna pogonski moment
% kao funkciju greske brzine, pozicije i integrala pozicije.
% Konstante P, I, i D dejstva se zadaju preko globalnih varijabli
% Pgain, Igain i Dgain respektivno. Zeljena trajektorija se zadaje
% putem globalne matrice qt= [t q_dmd qd_dmd ]
% Globalno se zadaje i pocetni uslov za integralno dejstvo ei.
% Primer poziva:
% >> qt= [t q_dmd qd_dmd];
% >> Pgain=[...]; Dgain=[...]; Igain=[...];
% >> ei=[0 0 0 0 0 0];
% >> global qt Pgain Dgain Igain ei
% >> [tsim, q, qd] = fdyn (dyn, to, tf, 'PIDctl')
%
% (c) 1996. Rasic Milan
function tau= pdctl (t, x)
   global Pgain Dgain qt;

 
  q= x(1:6)'; qd= x(7:12)';   % stanje robota
  
% odrazavanje vremena u zadatim granicama
  if t>qt(length(qt),1),
     t=qt(length(qt),1);
  end;


% interpolacija zeljenih vrednosti pozicije i brzine
   q_dmd = interp1 (qt(:,1), qt(:,2:7), t);
   qd_dmd = interp1(qt(:,1), qt(:,8:13), t);

% izracunavanje greske
   e  = q_dmd - q;
   ed = qd_dmd - qd;
  
% izracunavanje upravljackog momenta
   tau = e * diag(Pgain) + ed * diag(Dgain);

