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

Compare commits

base: a2nr:master
Branches Tags
a2nr:master
a2nr:v1.0
..
compare: a2nr:v1.0
Branches Tags
a2nr:master
a2nr:v1.0

No commits in common. "master" and "v1.0" have entirely different histories.
master ... v1.0

3 changed files with 0 additions and 179 deletions
Show Stats Expand all files Collapse all files

129
SOURCE/SM_2015_Rozenheck_PLOT_MOVE.m
View File

@ -1,129 +0,0 @@
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

BIN
SOURCE/output/output_x001002_y003-03_kp1050_kp2003.mpeg
View File

Binary file not shown.

50
SOURCE/plot_move.m
View File

@ -1,50 +0,0 @@
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