33 lines
945 B
Matlab
33 lines
945 B
Matlab
|
|
%% 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
|