23 lines
673 B
Matlab
23 lines
673 B
Matlab
function dxdt = systm_anlys_robot(t,x,xRobot, C, ppC,d,R,K,kp,ki,B,vref)
|
|
n = length(K(1,:))/2;
|
|
m = length(K(:,1))/2;
|
|
%% l = length(R(ones(n*2,1),K));
|
|
dxdt = zeros(m*2,1);
|
|
x1 = x(1:m);
|
|
x2 = x(length(x1)+1:length(x));
|
|
counter = C(0);
|
|
%% length(x1)
|
|
%% length(x2)
|
|
%% if (t > 5) && (t < 15)
|
|
%% vref = [5; 10];
|
|
%% else
|
|
%% if (t > 15) && (t > 20)
|
|
%% vref = [5; -10];
|
|
%% endif
|
|
%% endif
|
|
%% size(xRobot)
|
|
dxdt(1:length(x1)) = -(2*kp*(R(xRobot(counter,:)',K)*R(xRobot(counter,:)',K)')*x1) -(2*(R(xRobot(counter,:)',K)*R(xRobot(counter,:)',K)')*x2) +((2*R(xRobot(counter,:)',K)*B)*vref);
|
|
dxdt(length(x1)+1:length(x)) =(ki*x1);
|
|
ppC();
|
|
endfunction
|