tambah 1st builder

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;
+kp = 1;
-ki = 3;
+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(sys.R(corRobot,sys.K),1),1);
-_zero = zeros(size(R(corRobot,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 = s2ndModelInit;
-% state_init = s1stInit;
+state_init = s1stInit;
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
 printf("Mulai memecakan masalah \n\n");
 startExe = tic;
 
 % continue
-% dydt = @(t, y) sys.ss(y,fvref(t));
+dydt = @(t, y) sys.ss(y,fvref(t));
-% [t,y] = ode45(dydt, tspan, state_init);
+[t,y] = ode45(dydt, tspan, state_init);
 
 % discrate taylor methode
-dydt = @(t, y) sys.dss(y, h, fvref(t))
+% dydt = @(t, y) sys.dss(y, h, fvref(t))
-t(1,:) = tspan(1);
+% t(1,:) = tspan(1);
-y(1,:) = dydt(t(1), state_init); 
+% 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))]';
+% 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);
+  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))]';
-  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))]';
 endfor
 
 endExe = toc(startExe);

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

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)));