a2nr
/
FormationControlSimulation
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

tambah 1st builder

master
a2nr 2021-01-29 10:29:16 +07:00
parent d55d7c2a5c
commit b17e689911
4 changed files with 55 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
SOURCE/SM_2015_Rozenheck.m
View File

@ -7,7 +7,7 @@ conRobot = [1 2 1;
3 2 1;
3 1 1;
];
length_d = .5;
length_d = 3;
dRobot = [ 1;
1;
1;
@ -22,8 +22,8 @@ B = [
0 0; 0 0;
0 0; 0 0;
];
kp = 150;
ki = 3;
kp = 1;
ki = 0;
% fvref = fncSpeedRef('ysin',50,2,robStateLen);
% fvref = fncSpeedRef('xsin',50,2,robStateLen);
% fvref = fncSpeedRef('cw',-300,1,robStateLen);
@ -31,44 +31,44 @@ ki = 3;
fvref = fncSpeedRef('s',0,0,robStateLen);
h = .01;
tspan = 1:h:10;
sys = formationControl2ndBuilder(conRobot,robStateLen,kp,kp,ki,ki,length_d,1);
% sys = formationControl2ndBuilder(conRobot,robStateLen,kp,kp,ki,ki,length_d,1);
% sys = formationControl1stBuilder(conRobot,robStateLen,kp,kp,ki,ki,length_d,1);
% sys = formationControl2ndModelBuilder(conRobot,omni.origin,kp,kp,ki,ki,length_d,1);
% sys = formationControl1stModelBuilder(conRobot,omni.origin,kp,kp,ki,ki,length_d,1);
sys = formationControl1stModelBuilder(conRobot,omni.origin,kp,kp,ki,ki,length_d,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
[R,K,d] = rigidityMatrixFnc(conRobot,robStateLen);
_zero = zeros(size(R(corRobot,K),1),1);
_zero = zeros(size(sys.R(corRobot,sys.K),1),1);
sInit = [corRobot; _zero;];
sAnsInit =[_zero; _zero;];
s2ndInit = [corRobot; %x1
zeros(length(corRobot),1); %x2
zeros(length(corRobot),1); %xi1
_zero;]; %xi2
% _zero; zeros(length(corRobot),1);]; %x2*
s1stInit = [corRobot; zeros(size(corRobot)); zeros(size(dRobot))];
s2ndInit = [corRobot;zeros(length(corRobot),1); zeros(length(corRobot),1); _zero;];
s2ndModelInit = [corRobot; zeros(length(corRobot),1); zeros(length(corRobot),1); _zero; zeros(length(corRobot),1);];
% state_init = s2ndInit;
% state_init = sInit;
state_init = s2ndInit;
% state_init = s1stInit;
% state_init = s2ndModelInit;
state_init = s1stInit;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
printf("Mulai memecakan masalah \n\n");
startExe = tic;
% continue
% dydt = @(t, y) sys.ss(y,fvref(t));
% [t,y] = ode45(dydt, tspan, state_init);
dydt = @(t, y) sys.ss(y,fvref(t));
[t,y] = ode45(dydt, tspan, state_init);
% discrate taylor methode
dydt = @(t, y) sys.dss(y, h, fvref(t))
t(1,:) = tspan(1);
y(1,:) = dydt(t(1), state_init);
Dd(1,:) = [norm((K*y(1,1:9)')(1:3)) norm((K*y(1,1:9)')(4:6)) norm((K*y(1,1:9)')(7:9))]';
% dydt = @(t, y) sys.dss(y, h, fvref(t))
% t(1,:) = tspan(1);
% y(1,:) = dydt(t(1), state_init);
% for i = 2:length(tspan)
% t(i,:) = tspan(i);
% y(i,:) = dydt(t(i),y(i-1,:)');
% endfor
Dd(1,:) = [norm((sys.K*y(1,1:9)')(1:3)) norm((sys.K*y(1,1:9)')(4:6)) norm((sys.K*y(1,1:9)')(7:9))]';
for i = 2:length(tspan)
t(i,:) = tspan(i);
y(i,:) = dydt(t(i),y(i-1,:)');
Dd(i,:) = [norm((K*y(i,1:9)')(1:3)) norm((K*y(i,1:9)')(4:6)) norm((K*y(i,1:9)')(7:9))]';
Dd(i,:) = [norm((sys.K*y(i,1:9)')(1:3)) norm((sys.K*y(i,1:9)')(4:6)) norm((sys.K*y(i,1:9)')(7:9))]';
endfor
endExe = toc(startExe);

30
SOURCE/formationControl1stBuilder.m Normal file
View File

@ -0,0 +1,30 @@
function obj = formationControl2ndBuilder(edgeL,robStateLen,kp1,kp2,ki1,ki2,dScale,nNodeVref)
addpath("./networks-toolbox");
obj = struct();
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
obj.m = length(obj.d);
obj.d = obj.d *dScale;
obj.n = numNodes(edgeL);
obj.nodeStateLength = robStateLen*obj.n;
obj.stateLength = 3 * obj.nodeStateLength + obj.m;
%build matrix system
obj.A = @(st) [-kp1*obj.R(st(1:(obj.nodeStateLength)),obj.K)'*obj.R(st(1:(obj.nodeStateLength)),obj.K) obj.R(st(1:(obj.nodeStateLength)),obj.K)'; ki1*obj.R(st(1:(obj.nodeStateLength)),obj.K) zeros(obj.m, obj.n)];
obj.sizeA = size(obj.A(ones(obj.stateLength,1)));
obj.B = @(st) [kp1*obj.R(st(1:(obj.nodeStateLength)),obj.K)'; -ki1*ones(obj.m)];
obj.sizeB = size(obj.B(ones(obj.stateLength,1)));
obj.dss = @(st,h,vRef) (eye(obj.sizeA) + (obj.A(st)*h))*st + (obj.B(st)*h)*obj.d ;
obj.ss = @(st,vRef) obj.A(st)*st + obj.B(st)*obj.d ;
printf("============== Formation Control ============ \n");
printf(" state model : %i \n", obj.nodeStateLength);
printf(" Node : %i \n", obj.n);
printf(" Edge : %i \n", obj.m);
printf("Dimention State : %i \n", obj.stateLength);
printf(" Dimention A : %i x %i \n", obj.sizeA);
printf(" Dimention B : %i x %i \n", obj.sizeB);
printf("============================================= \n");
endfunction

2
SOURCE/formationControl1stModelBuilder.m
View File

@ -13,7 +13,7 @@ function obj = formationControl1stModelBuilder(edgeL,model,kp1,kp2,ki1,ki2,dScal
% pilih robot mana yang ingin dijalankan dengan mengganti 0 menjadi 1
% r 1 2 3 r 1 2 3
% v v v v v v
obj.A = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) diag([1 0 0])],model.A) + kron([zeros(obj.n) diag([1 0 0]);zeros(obj.n) zeros(obj.n)],eye(obj.n));
obj.A = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) diag([1 1 1])],model.A) + kron([zeros(obj.n) diag([1 1 1]);zeros(obj.n) zeros(obj.n)],eye(obj.n));
obj.B = kron([zeros(obj.n); eye(obj.n)],model.B);
obj.Ksgn = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) eye(obj.n)],model.K);
obj.controller = @(set_point,st) arrayfun(model.limit_motor,_nst * set_point + _kst * st(1:(2 * obj.nodeStateLength)));

BIN
octave-workspace Normal file
View File

Binary file not shown.