18 lines
528 B
Matlab
18 lines
528 B
Matlab
function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
|
|
l = length(x);
|
|
n = length(K(1,:))/2;
|
|
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)')*(ones(size(R(x1,K)',2),1)*d))+(B*vref);
|
|
dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*ones(m,1))*d;
|
|
endfunction
|