second order builder done

.gitignore

@@ -0,0 +1,2 @@
+*.data
+*.csv

SOURCE/DataErrorEdge.data

@@ -1,48 +0,0 @@
-# Created by Octave 5.1.0, Tue Dec 17 11:11:50 2019 WIB <adnr@morilin.fedo>
-# name: yans
-# type: matrix
-# rows: 41
-# columns: 20
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
-
-

SOURCE/SM_2015_Rozenheck.m

@@ -1,4 +1,5 @@
 clear -all
+graphics_toolkit('gnuplot')
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
 conRobot = [1 2 1;
             3 2 1;
@@ -23,9 +24,11 @@ ki = 1;
 % fvref = fncSpeedRef('ysin',50,2);
 % fvref = fncSpeedRef('xsin',50,2);
 % fvref = fncSpeedRef('cw',-300,1);
-fvref = fncSpeedRef('s',50,50);
+% fvref = fncSpeedRef('s',50,50);
-fvrefans = fncSpeedRef('s',0,0);
+fvref = fncSpeedRef('s',0,0);
-tspan = 1:0.1:10;
+h = .1;
+tspan = 1:h:10;
+sys = formationControl2ndBuilder(conRobot,kp,kp,ki,ki,length_d,1);
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
 
 [R,K,d] = rigidityMatrixFnc(conRobot);
@@ -41,14 +44,8 @@ s2ndInit = [corRobot;     %x1
 printf("Mulai memecakan masalah \n\n");
 startExe = tic;
 
-dydt = @(t, y) systm_2nd_order_robot(t, y,(dRobot*length_d), R,K, kp,kp, ki,ki, B, fvref(t));
+dydt = @(t, y) sys.ss(y,fvref(t));
 [t,y] = ode45(dydt, tspan, s2ndInit);
-cntr = 1;
-bypassCntr = @(c) cntr;
-plusPlusCntr = @(c) cntr++;
-
-dyansdt = @(t, yin) systm_anlys_robot(t, yin, y(:,1:(2*numNodes(edgeL2adj(conRobot)))) ,bypassCntr, plusPlusCntr ,(dRobot*6), R, K, kp, ki, B, fvref(t));
-[t,yans] = ode45(dyansdt, tspan, sAnsInit);
 
 endExe = toc(startExe);
 printf("Membutuhkan waktu %i menit, %i detik \n untuk memecahkan masalah mu  \n\n",
@@ -108,33 +105,7 @@ str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
 eval(str_tmp)
 title("Motion dari Robot");
 
-figure(2)
+csvwrite("DataOutMotion.csv", [t y])
-                                % Plot analisis error secara keseluruhan 
-ddYans = zeros(size(yans,1),1);
-for i = 1:size(yans,1)
-  ddYans(i) = norm(yans(i,1:(size(yans,2)/2)));
-endfor
-
-plot([1:length(ddYans)],ddYans )
-hold on
-                                % Plot analisis error setiap robot
-str_tmp = "plot(";
-for i = 1:length(dRobot)-1
-  str_tmp = strcat(str_tmp,sprintf("[1:length(yans'(%i,:))], yans'(%i,:),",i,i));
-endfor
-str_tmp = strcat(str_tmp,sprintf("[1:length(yans'(%i,:))], yans'(%i,:));",i+1,i+1));
-eval(str_tmp);
-
-str_tmp = "legend(";
-str_tmp = strcat(str_tmp,sprintf("\"All\", "));
-for i = 1:numNodes(edgeL2adjL(conRobot))-1;
-  str_tmp =strcat(str_tmp,sprintf("\"R%i \", ",i));
-endfor
-str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
-eval(str_tmp)
-title("Norm error setiap edge robot")
-
-csvwrite("DataOutMotion.csv", [t' y'])
 % save DataOutMotion.data y
 % save DataErrorEdge.data yans
 

SOURCE/formationControl2ndBuilder.m

@@ -0,0 +1,37 @@
+function obj = formationControl2ndBuilder(edgeL,kp1,kp2,ki1,ki2,dScale,nNodeVref)
+    addpath("./networks-toolbox");
+    obj = struct();
+    [obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL);
+    obj.m = length(obj.d);
+    obj.n = numNodes(edgeL);
+    obj.stateLength = 3 * 2*obj.n + obj.m;
+
+    % build matrix sistem
+    obj.A1 = @(st) [zeros(2*obj.n), eye(2*obj.n), zeros(2*obj.n), zeros(obj.n*2,obj.m)];
+    obj.A2 = @(st) [-kp2*obj.R(st(1:(2*obj.n)),obj.K)'*obj.R(st(1:(2*obj.n)),obj.K), -kp1*eye(2*obj.n), -eye(2*obj.n), -obj.R(st(1:(2*obj.n)),obj.K)'];
+    obj.A3 = @(st) [zeros(2*obj.n), ki1*eye(2*obj.n), zeros(2*obj.n), zeros(obj.n*2,obj.m)];
+    obj.A4 = @(st) [ki2*obj.R(st(1:(2*obj.n)),obj.K), zeros(obj.m,obj.n*2), zeros(obj.m,obj.n*2), zeros(obj.m)];
+    obj.A = @(st) [obj.A1(st); obj.A2(st); obj.A3(st); obj.A4(st)];
+    obj.sizeA = size(obj.A(ones(obj.stateLength,1)));
+
+    % build matrix input
+    obj.B = @(st) [zeros(2*obj.n,obj.m); kp2*obj.R(st(1:(2*obj.n)),obj.K)'; zeros(2*obj.n,obj.m); -ki2*eye(obj.m,obj.m)]
+    obj.sizeB = size(obj.B(ones(obj.stateLength,1)));
+
+    %build state space
+    obj.Kb = zeros(obj.n*3,1);
+    obj.Kb(obj.n+nNodeVref) = 1;
+    obj.Kb = [kron(obj.Kb,eye(2)); zeros(obj.m,2)];
+    obj.d = obj.d*dScale;
+    obj.ssDisc = @(st,vRef,h) (eye(obj.sizeA) + (obj.A(st)*h))*st + obj.B(st)*h*obj.d + obj.Kb*vRef;
+    obj.ss = @(st,vRef) obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;
+
+    printf("============== Formation Control ============ \n");
+    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/systm_2nd_order_robot.m

@@ -4,10 +4,10 @@
 %% t -> time
 %% x -> state
 %% d -> distance antara robot nya
-%% R, K -> return dari fungsi rigidityMatrixFnc()
+%% r, k -> return dari fungsi rigiditymatrixfnc()
 %% kp -> konstanta proporsional
 %% ki -> konstanta integral
-%% B -> matrix selctor vref
+%% b -> matrix selctor vref
 %% vref -> kecepatan refrensi
 function dxdt = systm_2nd_order_robot(t,x,d,R,K,kp1,kp2,ki1,ki2,B,vref)
   n = length(K(1,:))/2;