21 lines
619 B
Matlab
21 lines
619 B
Matlab
%% systm_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_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));
|
|
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
|