141 lines
4.0 KiB
Matlab
141 lines
4.0 KiB
Matlab
clear -all
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
|
|
conRobot = [1 2 1;
|
|
3 2 1;
|
|
3 1 1;
|
|
];
|
|
length_d = .5;
|
|
dRobot = [ 1;
|
|
1;
|
|
1;
|
|
];
|
|
corRobot = [1.5; 1.7;
|
|
2; 2.5;
|
|
2.5; 2.8;
|
|
]; % xy xy xy
|
|
B = [
|
|
1 0; 0 1;
|
|
0 0; 0 0;
|
|
0 0; 0 0;
|
|
];
|
|
kp = 80;
|
|
ki = 1;
|
|
% fvref = fncSpeedRef('ysin',50,2);
|
|
% fvref = fncSpeedRef('xsin',50,2);
|
|
% fvref = fncSpeedRef('cw',-300,1);
|
|
fvref = fncSpeedRef('s',50,50);
|
|
fvrefans = fncSpeedRef('s',0,0);
|
|
tspan = 1:0.1:10;
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
|
|
|
|
[R,K,d] = rigidityMatrixFnc(conRobot);
|
|
_zero = zeros(size(R(corRobot,K),1),1);
|
|
sInit = [corRobot; _zero;];
|
|
sAnsInit =[_zero; _zero;];
|
|
s2ndInit = [corRobot; %x1
|
|
zeros(length(corRobot),1); %x2
|
|
zeros(length(corRobot),1); %xi1
|
|
_zero]; %xi2
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
|
|
printf("Mulai memecakan masalah \n\n");
|
|
startExe = tic;
|
|
|
|
dydt = @(t, y) systm_2nd_order_robot(t, y,(dRobot*length_d), R,K, kp,kp, ki,ki, B, fvref(t));
|
|
[t,y] = ode45(dydt, tspan, s2ndInit);
|
|
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, fvref(t));
|
|
[t,yans] = ode45(dyansdt, tspan, sAnsInit);
|
|
|
|
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(";
|
|
for i = 1:length(corRobot)-1
|
|
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),",i));
|
|
endfor
|
|
str_tmp = strcat(str_tmp,sprintf("y'(%i,:));",i+1));
|
|
eval(str_tmp);
|
|
|
|
hold on
|
|
% membuat fungsi plot robot di waktu
|
|
% tertentu
|
|
str_tmp = "@(t) plot( [";
|
|
for i = 1:2:length(corRobot)
|
|
str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
|
|
endfor
|
|
str_tmp = strcat( str_tmp, sprintf("], ["));
|
|
for i = 2:2: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:2:length(corRobot);
|
|
yrb = 2:2: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', conRobot, xrb, yrb, 1);
|
|
% plot_con(plot_rb, y', conRobot, xrb, yrb, round(length(tspan)/2));
|
|
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("\"R%i \", ",i));
|
|
endfor
|
|
str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
|
|
eval(str_tmp)
|
|
title("Motion dari Robot");
|
|
|
|
figure(2)
|
|
% Plot analisis error secara keseluruhan
|
|
ddYans = zeros(size(yans,1),1);
|
|
for i = 1:size(yans,1)
|
|
ddYans(i) = norm(yans(i,1:(size(yans,2)/2)));
|
|
endfor
|
|
|
|
plot([1:length(ddYans)],ddYans )
|
|
hold on
|
|
% Plot analisis error setiap robot
|
|
str_tmp = "plot(";
|
|
for i = 1:length(dRobot)-1
|
|
str_tmp = strcat(str_tmp,sprintf("[1:length(yans'(%i,:))], yans'(%i,:),",i,i));
|
|
endfor
|
|
str_tmp = strcat(str_tmp,sprintf("[1:length(yans'(%i,:))], yans'(%i,:));",i+1,i+1));
|
|
eval(str_tmp);
|
|
|
|
str_tmp = "legend(";
|
|
str_tmp = strcat(str_tmp,sprintf("\"All\", "));
|
|
for i = 1:numNodes(edgeL2adjL(conRobot))-1;
|
|
str_tmp =strcat(str_tmp,sprintf("\"R%i \", ",i));
|
|
endfor
|
|
str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
|
|
eval(str_tmp)
|
|
title("Norm error setiap edge robot")
|
|
|
|
csvwrite("DataOutMotion.csv", [t' y'])
|
|
% save DataOutMotion.data y
|
|
% save DataErrorEdge.data yans
|
|
|