49 lines
1.6 KiB
Matlab
49 lines
1.6 KiB
Matlab
% fungsi untuk plot coneksi di waktu
|
|
% tertentu
|
|
function plot_con (yOut, conIn, time, bParam)
|
|
lengthCorRobot = length(bParam.corRobot);
|
|
robStateLen = size(bParam.model.origin.A,1);
|
|
lengthNode = length(bParam.edgeL);
|
|
tspan = bParam.tspan;
|
|
|
|
xi = 1:robStateLen:lengthCorRobot-1;
|
|
yi = 2:robStateLen:lengthCorRobot;
|
|
% plot trayektori dari setiap robot
|
|
str_tmp = "plot(";
|
|
for i = 1:lengthNode-1
|
|
str_tmp = strcat(str_tmp,sprintf("yOut(%i,:),yOut(%i,:),",xi(i),yi(i)));
|
|
endfor
|
|
str_tmp = strcat(str_tmp,sprintf("yOut(%i,:),yOut(%i,:));",xi(i+1),yi(i+1)));
|
|
eval(str_tmp);
|
|
hold on
|
|
% membuat fungsi plot robot di waktu
|
|
% tertentu
|
|
str_tmp = "@(t) plot( [";
|
|
for i = 1:robStateLen:lengthCorRobot
|
|
str_tmp = strcat( str_tmp, sprintf("yOut(%i,t), ",i));
|
|
endfor
|
|
str_tmp = strcat( str_tmp, sprintf("], ["));
|
|
for i = 2:robStateLen:lengthCorRobot
|
|
str_tmp = strcat( str_tmp, sprintf("yOut(%i,t), ",i));
|
|
endfor
|
|
str_tmp = strcat( str_tmp, sprintf("], \"^\");"));
|
|
plot_rb = eval(str_tmp);
|
|
|
|
xrb = 1:robStateLen:lengthCorRobot;
|
|
yrb = 2:robStateLen:lengthCorRobot;
|
|
|
|
for tm = 1:length(time)
|
|
plot_rb(time(tm));
|
|
for i = 1:length(conIn)
|
|
plot([yOut(xrb(conIn(i, 1)),time(tm)), yOut(xrb(conIn(i, 2)),time(tm))],
|
|
[yOut(yrb(conIn(i,1)),time(tm)), yOut(yrb(conIn(i,2)),time(tm))], "r" )
|
|
endfor
|
|
endfor
|
|
|
|
str_tmp = "legend(";
|
|
for i = 1:numNodes(edgeL2adjL(bParam.edgeL))-1;
|
|
str_tmp =strcat(str_tmp,sprintf("\"R%i \", ",i));
|
|
endfor
|
|
str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
|
|
eval(str_tmp)
|
|
endfunction |