diff --git a/SOURCE/SM_2015_Rozenheck.m b/SOURCE/SM_2015_Rozenheck.m index 8448cf9..8461687 100644 --- a/SOURCE/SM_2015_Rozenheck.m +++ b/SOURCE/SM_2015_Rozenheck.m @@ -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); diff --git a/SOURCE/formationControl1stBuilder.m b/SOURCE/formationControl1stBuilder.m new file mode 100644 index 0000000..37c92f5 --- /dev/null +++ b/SOURCE/formationControl1stBuilder.m @@ -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 \ No newline at end of file diff --git a/SOURCE/formationControl1stModelBuilder.m b/SOURCE/formationControl1stModelBuilder.m index c0e1770..d45c332 100644 --- a/SOURCE/formationControl1stModelBuilder.m +++ b/SOURCE/formationControl1stModelBuilder.m @@ -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))); diff --git a/octave-workspace b/octave-workspace new file mode 100644 index 0000000..8a2c82e Binary files /dev/null and b/octave-workspace differ