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

second order builder done

master
a2nr 2020-07-28 13:32:25 +07:00
parent 02f9ae1bbf
commit 64e56ec5d8
7 changed files with 49 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored Normal file
View File

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

BIN
DOC/RP_2015_Rozenheck.pdf
View File

Binary file not shown.

48
SOURCE/DataErrorEdge.data
View File

@ -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

45
SOURCE/SM_2015_Rozenheck.m
View File

@ -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);
fvrefans = fncSpeedRef('s',0,0);
tspan = 1:0.1:10;
% fvref = fncSpeedRef('s',50,50);
fvref = fncSpeedRef('s',0,0);
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)
% 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'])
csvwrite("DataOutMotion.csv", [t y])
% save DataOutMotion.data y
% save DataErrorEdge.data yans

37
SOURCE/formationControl2ndBuilder.m Normal file
View File

@ -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

BIN
SOURCE/octave-workspace
View File

Binary file not shown.

4
SOURCE/systm_2nd_order_robot.m
View File

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