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

Update support plot analisis

master
a2nr 2019-07-10 13:34:08 +07:00
parent f60517a513
commit a0faa211e1
5 changed files with 86 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
SOURCE/SM_2015_Rozenheck.m
View File

@ -1,6 +1,5 @@
clear -all
close all
%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1; 5 3 1; 5 1 1;6 4 1; 6 2 1; 5 6 1];
conRobot = [1 2 1;
1 4 1;
1 5 1;
@ -9,10 +8,31 @@ conRobot = [1 2 1;
2 4 1;
3 4 1;
4 5 1;
5 6 1;];
%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1;];
%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1; -1; -1; 0; -1; ]; % xy xy xy
5 6 1;
5 2 1;
%% 6 3 1;
%% 6 4 1;
%% 6 2 1;
%% 3 1 1;
%% 3 5 1;
];
dRobot = [ 1;
1;
1;
1;
1;
1;
1;
1;
1;
1;
%% 1;
%% 2;
%% 2;
%% 2;
%% 2;
%% 2;
];
corRobot = [2; 2.5;
2.7; 2.6;
3; 2.3;
@ -20,10 +40,8 @@ corRobot = [2; 2.5;
1.8; 1.5;
1.5; 2;
]; % xy xy xy
%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1;]; % xy xy xy
d = 3;
%% figure(1)
[R,K] = rigidityMatrixFnc(conRobot);
[R,K,d] = rigidityMatrixFnc(conRobot);
vref = [-5; 5];
B = [
1 0; 0 1;
@ -33,16 +51,30 @@ B = [
0 0; 0 0;
0 0; 0 0;
];
kp = 2;
kp = 10;
ki = 3;
sInit = [corRobot; zeros(size(R(corRobot,K),1),1)];
_zero = zeros(size(R(corRobot,K),1),1);
sInit = [corRobot; _zero;];
sAnsInit =[_zero; _zero;];
%% tspan = linspace(1,0.1,30);
tspan = 1:0.01:5;
dydt = @(t, y) systm_robot(t, y,d, R,K, kp, ki, B, vref);
tspan = 1:0.01:10;
dydt = @(t, y) systm_robot(t, y,(dRobot*6), R,K, kp, ki, B, vref);
[t,y] = ode45(dydt, tspan, sInit);
%% figure(2)
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, vref);
[t,yans] = ode45(dyansdt, tspan, sAnsInit);
ddYans = zeros(size(yans,1),1);
for i = 1:size(yans,1)
ddYans(i) = norm(yans(i,1:(size(yans,2)/2)));
endfor
figure(1)
str_tmp = "plot(";
for i = 1:length(corRobot)-1
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),",i));
@ -76,6 +108,17 @@ endfunction
plot_con(plot_rb, y', conRobot, xrb, yrb, 1);
plot_con(plot_rb, y', conRobot, xrb, yrb, length(tspan));
str_tmp = "legend(";
for i = 1:numNodes(edgeL2adjL(conRobot))-1;
str_tmp =strcat(str_tmp,sprintf("\"Robot %i \", ",i));
endfor
str_tmp =strcat(str_tmp,sprintf("\"Robot %i \" )",++i));
eval(str_tmp)
figure(2)
plot([1:length(ddYans)],ddYans )
%% plot_rb(20);
%% plot_rb(40);
%% plot_rb(60);

BIN
SOURCE/octave-workspace
View File

Binary file not shown.

4
SOURCE/rigidityMatrixFnc.m
View File

@ -1,10 +1,11 @@
function [R,K] = rigidityMatrixFnc (edgeL)
function [R,K, d] = rigidityMatrixFnc (edgeL)
%% Fungsi ini digunakan untuk menggenerasi fungsi rigidity R.
%% diaman fungsi tersebut adalah hasil turunan dari fungsi edge
addpath("./networks-toolbox");
pkg load symbolic
% generasi koneksi bentuk incidence
tmp = edgeL2adj(edgeL);
d = ones(numEdges(tmp),1);
gInc = adj2inc(tmp);
%% drawCircGraph(tmp)
tmp = getNodes(gInc,'adjacency')
@ -21,4 +22,5 @@ function [R,K] = rigidityMatrixFnc (edgeL)
% return sebagai fungsi R
eval(str)
R = @(x,k) errBlockDiagonal(edgeVector(x,k))*k;
endfunction

22
SOURCE/systm_anlys_robot.m Normal file
View File

@ -0,0 +1,22 @@
function dxdt = systm_anlys_robot(t,x,xRobot, C, ppC,d,R,K,kp,ki,B,vref)
n = length(K(1,:))/2;
m = length(K(:,1))/2;
%% l = length(R(ones(n*2,1),K));
dxdt = zeros(m*2,1);
x1 = x(1:m);
x2 = x(length(x1)+1:length(x));
counter = C(0);
%% length(x1)
%% length(x2)
%% if (t > 5) && (t < 15)
%% vref = [5; 10];
%% else
%% if (t > 15) && (t > 20)
%% vref = [5; -10];
%% endif
%% endif
%% size(xRobot)
dxdt(1:length(x1)) = -(2*kp*(R(xRobot(counter,:)',K)*R(xRobot(counter,:)',K)')*x1) -(2*(R(xRobot(counter,:)',K)*R(xRobot(counter,:)',K)')*x2) +((2*R(xRobot(counter,:)',K)*B)*vref);
dxdt(length(x1)+1:length(x)) =(ki*x1);
ppC();
endfunction

6
SOURCE/systm_robot.m
View File

@ -1,6 +1,6 @@
function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
l = length(x);
n = length(K(1,:))/2;
l = length(R(ones(n*2,1),K));
m = length(K(:,1))/2;
dxdt = zeros(l,1);
x1 = x(1:n*2);
@ -12,6 +12,6 @@ function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
%% vref = [5; -10];
%% endif
%% endif
dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*(ones(size(R(x1,K)',2),1)*d))+(B*vref);
dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*ones(m,1))*d;
dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*d);
endfunction