158 lines
4.5 KiB
Matlab
158 lines
4.5 KiB
Matlab
clear -all
|
|
|
|
graphics_toolkit('gnuplot')
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
|
|
algo = 0;
|
|
d = [[]];
|
|
ang = [];
|
|
save_state = [];
|
|
save algo_info.m algo;
|
|
save algo_info.m "-append" d ang save_state;
|
|
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.kp1 = 80;
|
|
bParam.kp2 = 7;
|
|
bParam.ki1 = 0;
|
|
bParam.ki2 = 0;
|
|
bParam.nNodeVref = 1;
|
|
|
|
|
|
corRobot = [0; 0;0;
|
|
1; 2;0;
|
|
3; -3;0;
|
|
]; % xy xy xy
|
|
robStateLen = size(bParam.model.origin.A,1);
|
|
B = [
|
|
1 0; 0 1;
|
|
0 0; 0 0;
|
|
0 0; 0 0;
|
|
];
|
|
|
|
% fvref = fncSpeedRef('ysin',50,2,robStateLen);
|
|
% fvref = fncSpeedRef('xsin',50,2,robStateLen);
|
|
% fvref = fncSpeedRef('cw',-300,1,robStateLen);
|
|
% fvref = fncSpeedRef('s',50,50,robStateLen);
|
|
fvref = fncSpeedRef('s',0,0,robStateLen);
|
|
h = .01;
|
|
tspan = 1:h:20;
|
|
|
|
## sys = formationControl2ndBuilder(bParam);
|
|
% sys = formationControl1stBuilder(bParam);
|
|
sys = formationControl2ndModelBuilder(bParam);
|
|
% sys = formationControl1stModelBuilder(bParam);
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
|
|
|
|
_zero = zeros(size(sys.R(corRobot,sys.K),1),1);
|
|
sInit = [corRobot; _zero;];
|
|
s1stInit = [corRobot; zeros(size(corRobot)); zeros(size(sys.d))];
|
|
s2ndInit = [corRobot;zeros(length(corRobot),1); zeros(length(corRobot),1); _zero;];
|
|
s2ndModelInit = [corRobot; zeros(length(corRobot),1); zeros(length(corRobot),1); _zero; zeros(length(corRobot),1);];
|
|
|
|
## state_init = s2ndInit;
|
|
% state_init = sInit;
|
|
state_init = s2ndModelInit;
|
|
% state_init = s1stInit;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
|
|
printf("Mulai memecakan masalah \n\n");
|
|
startExe = tic;
|
|
|
|
% continue
|
|
dydt = @(t, y) algo_init(sys,y,fvref(t));
|
|
##dydt = @(t, y) sys.ss(y,fvref(t));
|
|
[t,y] = ode45(dydt, tspan, state_init);
|
|
|
|
% discrate taylor methode
|
|
## dydt = @(t, y) sys.dss(y, h, fvref(t));
|
|
## t(1,:) = tspan(1);
|
|
## y(1,:) = dydt(t(1), state_init);
|
|
## for i = 2:length(tspan)
|
|
## t(i,:) = tspan(i);
|
|
## y(i,:) = dydt(t(i),y(i-1,:)');
|
|
## endfor
|
|
|
|
Dd(1,:) = [norm((sys.K*y(1,1:9)')(1:3)) norm((sys.K*y(1,1:9)')(4:6)) norm((sys.K*y(1,1:9)')(7:9))]';
|
|
for i = 2:length(tspan)
|
|
Dd(i,:) = [norm((sys.K*y(i,1:9)')(1:3)) norm((sys.K*y(i,1:9)')(4:6)) norm((sys.K*y(i,1:9)')(7:9))]';
|
|
endfor
|
|
|
|
endExe = toc(startExe);
|
|
printf("Membutuhkan waktu %i menit, %i detik \n untuk memecahkan masalah mu \n\n",
|
|
floor(endExe/60), rem(endExe,60))
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% End Solving
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start plot it
|
|
|
|
close all
|
|
|
|
figure(1)
|
|
|
|
% plot trayektori dari setiap robot
|
|
str_tmp = "plot(";
|
|
|
|
|
|
xi = 1:robStateLen:length(corRobot)-1
|
|
yi = 2:robStateLen:length(corRobot)
|
|
|
|
for i = 1:sys.n-1
|
|
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),y'(%i,:),",xi(i),yi(i)));
|
|
endfor
|
|
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),y'(%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:length(corRobot)
|
|
str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
|
|
endfor
|
|
str_tmp = strcat( str_tmp, sprintf("], ["));
|
|
for i = 2:robStateLen:length(corRobot)
|
|
str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
|
|
endfor
|
|
str_tmp = strcat( str_tmp, sprintf("], \"^\");"));
|
|
plot_rb = eval(str_tmp);
|
|
|
|
xrb = 1:robStateLen:length(corRobot);
|
|
yrb = 2:robStateLen:length(corRobot);
|
|
|
|
% fungsi untuk plot coneksi di waktu
|
|
% tertentu
|
|
function plot_con (pltRb, yOut, conIn,xm,ym, time)
|
|
pltRb(time);
|
|
for i = 1:length(conIn)
|
|
plot([yOut(xm(conIn(i, 1)),time), yOut(xm(conIn(i, 2)),time)],
|
|
[yOut(ym(conIn(i,1)),time), yOut(ym(conIn(i,2)),time)], "r" )
|
|
endfor
|
|
endfunction
|
|
|
|
plot_con(plot_rb, y', bParam.edgeL, xrb, yrb, 1);
|
|
% plot_con(plot_rb, y', bParam.edgeL, xrb, yrb, round(length(tspan)/2));
|
|
plot_con(plot_rb, y', bParam.edgeL, xrb, yrb, length(tspan));
|
|
|
|
|
|
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)
|
|
title("Motion dari Robot");
|
|
|
|
figure(2)
|
|
plot(t,Dd(:,1),t,Dd(:,2),t,Dd(:,3))
|
|
title("distance error")
|
|
legend()
|
|
|
|
csvwrite("DataOutMotion.csv", [t y(:,1:9)])
|
|
csvwrite("DataErrorEdge.csv", [t Dd])
|
|
save DataOutMotion.data y(:,1:9)
|
|
save DataErrorEdge.data Dd
|
|
|