add plot move
parent
442eeb5195
commit
0a6cb88a99
|
|
@ -0,0 +1,129 @@
|
|||
clear -all
|
||||
|
||||
graphics_toolkit('gnuplot')
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
|
||||
|
||||
bParam = struct();
|
||||
bParam.model = omnidirectional(0.94, 0.96, 0.01, 2.2, 1.5,0.099,0.1,1.5,0.025,30,0.035,19,0.00011,1.69,0.0059,0.0059,0.9,9.1,6,'./');
|
||||
bParam.edgeL = [1 2 1;3 2 1;3 1 1;];
|
||||
bParam.dScale = 3;
|
||||
bParam.nNodeVref = 1;
|
||||
bParam.ki1 = 0;
|
||||
bParam.ki2 = 0;
|
||||
bparam.hStep = .01;
|
||||
bParam.tspan = 1:bparam.hStep:20;
|
||||
# coba solo
|
||||
bParam.corRobot = [0; 0;0;1; 2;0;3; -3;0;]; % xy xy xy
|
||||
## CASE 1
|
||||
bParam.kp1 = 50;
|
||||
bParam.kp2 = 3;
|
||||
|
||||
bP(1,1) = bParam;
|
||||
# solo coba
|
||||
###### KOOR 1
|
||||
##bParam.corRobot = [0; 0;0;1; 2;0;-2; 3;0;]; % xy xy xy
|
||||
## ## CASE 1
|
||||
## bParam.kp1 = 50;
|
||||
## bParam.kp2 = 3;
|
||||
##
|
||||
## bP(1,1) = bParam;
|
||||
##
|
||||
## ## CASE 2
|
||||
## bParam.kp1 = 80;
|
||||
## bParam.kp2 = 7;
|
||||
##
|
||||
## bP(1,2) = bParam;
|
||||
##
|
||||
## # CASE 3
|
||||
## bParam.kp1 = 100;
|
||||
## bParam.kp2 = 15;
|
||||
##
|
||||
## bP(1,3) = bParam;
|
||||
##
|
||||
#### KOOR 2
|
||||
##bParam.corRobot = [0; 0;0;-2; -4;0;3; -2;0;]; % xy xy xy
|
||||
## ## CASE 1
|
||||
## bParam.kp1 = 50;
|
||||
## bParam.kp2 = 3;
|
||||
##
|
||||
## bP(2,1) = bParam;
|
||||
##
|
||||
## ## CASE 2
|
||||
## bParam.kp1 = 80;
|
||||
## bParam.kp2 = 7;
|
||||
##
|
||||
## bP(2,2) = bParam;
|
||||
##
|
||||
## # CASE 3
|
||||
## bParam.kp1 = 100;
|
||||
## bParam.kp2 = 15;
|
||||
##
|
||||
## bP(2,3) = bParam;
|
||||
##
|
||||
## KOOR 3
|
||||
##bParam.corRobot = [0; 0;0;1; 2;0;3; -3;0;]; % xy xy xy
|
||||
## ## CASE 1
|
||||
## bParam.kp1 = 50;
|
||||
## bParam.kp2 = 3;
|
||||
##
|
||||
## bP(3,1) = bParam;
|
||||
##
|
||||
## ## CASE 2
|
||||
## bParam.kp1 = 80;
|
||||
## bParam.kp2 = 7;
|
||||
##
|
||||
## bP(3,2) = bParam;
|
||||
##
|
||||
## # CASE 3
|
||||
## bParam.kp1 = 100;
|
||||
## bParam.kp2 = 15;
|
||||
##
|
||||
## bP(3,3) = bParam;
|
||||
##
|
||||
#### KOOR 4
|
||||
##bParam.corRobot = [0; 0;0;-2; -3;0;3; 2;0;]; % xy xy xy
|
||||
## ## CASE 1
|
||||
## bParam.kp1 = 50;
|
||||
## bParam.kp2 = 3;
|
||||
##
|
||||
## bP(4,1) = bParam;
|
||||
##
|
||||
## ## CASE 2
|
||||
## bParam.kp1 = 80;
|
||||
## bParam.kp2 = 7;
|
||||
##
|
||||
## bP(4,2) = bParam;
|
||||
##
|
||||
## # CASE 3
|
||||
## bParam.kp1 = 100;
|
||||
## bParam.kp2 = 15;
|
||||
##
|
||||
## bP(4,3) = bParam;
|
||||
|
||||
close all
|
||||
|
||||
for i = 1:size(bP,1)
|
||||
[t,y,Dd,DdNorm] = run_my_simulation(bP(i,1));
|
||||
|
||||
plot_move(y', length(bP(1,1).tspan), "/home/a2nr/Documents/FormationControlSimulation/SOURCE/output/",bP(i,1));
|
||||
|
||||
figure
|
||||
plot_con( y', bParam.edgeL, [1, length(bP(1,1).tspan)] ,bP(i,1));
|
||||
title(sprintf("Motion dari Robot Case %i ", i ));
|
||||
|
||||
## figure
|
||||
## resdd = struct();
|
||||
## resdd.DdNorm = DdNorm;
|
||||
## allresdd(1) = resdd;
|
||||
## for j = 2:size(bP,2)
|
||||
## [t,y,Dd,resdd.DdNorm] = run_my_simulation(bP(i,j));
|
||||
## allresdd(j) = resdd;
|
||||
## endfor
|
||||
## plot(t,allresdd(1).DdNorm,t,allresdd(2).DdNorm,t,allresdd(3).DdNorm);
|
||||
## title(sprintf("Norm Distance error Case %i ", i));
|
||||
endfor
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Binary file not shown.
|
|
@ -0,0 +1,50 @@
|
|||
function plot_move(yOut,time,path,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;
|
||||
str_tmp = "@(e) plot(";
|
||||
for i = 1:lengthNode-1
|
||||
str_tmp = strcat(str_tmp,sprintf("yOut(%i,1:e),yOut(%i,1:e),",xi(i),yi(i)));
|
||||
endfor
|
||||
str_tmp = strcat(str_tmp,sprintf("yOut(%i,1:e),yOut(%i,1:e));",xi(i+1),yi(i+1)));
|
||||
_plot_move = eval(str_tmp); %_plot_move(end);
|
||||
|
||||
|
||||
% 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); % plot_rb(coordinat)
|
||||
|
||||
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));
|
||||
|
||||
|
||||
for t=1:time
|
||||
f=figure('visible', 'off');
|
||||
hold on
|
||||
_plot_move(t);
|
||||
plot_rb(t);
|
||||
eval(str_tmp);
|
||||
filename=strcat(path,sprintf('x%03d%03d_y%03d%03d_kp1%03d_kp2%03d_%05d.png', ...
|
||||
bParam.corRobot(4),bParam.corRobot(5),bParam.corRobot(7),bParam.corRobot(8),...
|
||||
bParam.kp1,bParam.kp2,t));
|
||||
print(filename);
|
||||
hold off
|
||||
close(f);
|
||||
endfor
|
||||
endfunction
|
||||
Loading…
Reference in New Issue