FormationControlSimulation/SOURCE/systm_robot.m

function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
  n = length(K(1,:))/2;
  l = length(R(ones(n*2,1),K));
  m = length(K(:,1))/2;
  dxdt = zeros(l,1);
  x1 = x(1:n*2);
  x2 = x(length(x1)+1:length(x));
  %% if (t > 5) && (t < 15)
    %% vref = [5; 10];
  %% else
    %% if (t > 15) && (t > 20)
      %% vref = [5; -10];
    %% endif
  %% endif
  %% dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
  dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
  dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*d);
endfunction