%% systm_2nd_robot
%% param :
%% t -> time
%% x -> state
%% d -> distance antara robot nya
%% r, k -> return dari fungsi rigiditymatrixfnc()
%% kp -> konstanta proporsional
%% ki -> konstanta integral
%% b -> matrix selctor vref
%% vref -> kecepatan refrensi
function dxdt = systm_2nd_order_robot(t,x,d,R,K,kp1,kp2,ki1,ki2,B,vref)
  n = length(K(1,:))/2;
  l = length(R(ones(n*2,1),K));
  m = length(K(:,1))/2;
  dxdt = zeros(length(x),1);
  x1 = x(1:n*2);
  len = length(x1);
  x2 = x(len+1:len+length(x1));
  len = len + length(x2);
  x3 = x(len+1:len+length(x2));
  len = len + length(x3);
  x4 = x(len+1:length(x));
  
  dxdt(1:n*2)                = x2;
  len = length(x1);
  dxdt(len+1:len+length(x1)) = -kp1*x2 -kp2*R(x1,K)'*R(x1,K)*x1 +kp2*R(x1,K)'*d -x3 -R(x1,K)'*x4 + (B*vref);
  len = len + length(x2);
  dxdt(len+1:len+length(x2)) = ki1*x2;
  len = len + length(x3);
  dxdt(len+1:length(x))      = ki2*R(x1,K)*x1 -ki2*d;
endfunction