12 lines
456 B
Matlab
12 lines
456 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;
|
|
dxdt = zeros(m*2,1);
|
|
x1 = x(1:m);
|
|
x2 = x(length(x1)+1:length(x));
|
|
counter = C(0);
|
|
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
|