% FDYNACT  integracija direktne dinamike sa aktuatorima
%
%    [T Q QD] = FDYNACT(DYN, ACTDYN, T0, T1, VOLTFUN)
%    [T Q QD] = FDYNACT(DYN, ACTDYN, T0, T1, VOLTFUN, Q0, QD0)
%
%    Integracija kompletne dinamike manipulatora, tj mehanizma i 
%    aktuatora, na vremenskom intervalu, a rezultat je vektor pozicija i
%    brzina u zglobovima. Pogonski napon zadaje se korisnickom funkcijom
%          U = VOLTFUN (T, X)
%    gde je X vektor kolona 2n*1 [Q; QD], a T je tekuce vreme.    
%
%    vidi: FDYN, RNE, DYN, ACTDYN, ODE45
%
%    (c) 1996. Rasic Milan
% V2

function [t, q, qd]= fdynact (dyn, dynact, t0, t1, voltfun, q0, qd0)
   global FA_DYN FA_ACTDYN FA_VOLTFUN

   n= numrows(dyn);
   if nargin < 5,
      disp ('Nedostaju argumenti');
   elseif nargin == 5,
      FA_VOLTFUN = voltfun;
      x0 = zeros (2*n, 1);
   elseif (nargin >= 5),
      FA_VOLTFUN = voltfun;
      x0 = [ q0(:); qd0(:)];
   end
   
   FA_DYN = dyn;
   FA_ACTDYN = dynact;   

   [t,y] = ode23 ('fdynact2', t0, t1, x0);
   q  = y(:,1:n);
   qd = y(:, n+1:2*n);
    

