FormationControlSimulation/SOURCE/SM_2015_Rozenheck.m

clear -all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
conRobot = [1 2 1;
            %% 2 1 -1;
            1 5 1;
            %% 5 1 -1;
            1 6 1;
            %% 6 1 -1;
            2 3 1;
            %% 3 2 -1;
            2 4 1;
            %% 4 2 -1;
            3 4 1;
            %% 4 3 -1;
            4 5 1;
            %% 5 4 -1;
            5 6 1;
            %% 6 5 -1;
            1 4 1;
            %% 4 1 -1;
            5 2 1;
            %% 2 5 -1
            %% 6 3 1;
            %% 6 4 1;
            %% 6 2 1;
            %% 3 1 1;
            %% 3 5 1;
           ];
length_d = 3;
dRobot = [ 1;
           1;
           1;
           1;
           1;
           1;
           1;
           1;
           1.41;
           1.41;
           %% 2;
           %% 3.014;
           %% 3.014;
           %% 3.014;
           %% 3.014;
];
corRobot = [1.5; 1.7;
            2; 2.5;
            2.5; 2.8;
            2.5; 2;
            2.2; 1;
            1.5; 0.2;
           ]*10; % xy xy xy
B = [
     1 0; 0 1;
     0 0; 0 0;
     0 0; 0 0;
     0 0; 0 0;
     0 0; 0 0;
     0 0; 0 0;
    ];
kp = 30;
ki = 3;
%% fvref = fncSpeedRef('ysin',100,2);
%% fvref = fncSpeedRef('xsin',50,2);
%% fvref = fncSpeedRef('cw',-100,1);
fvref = fncSpeedRef('s',0,0);
fvrefans = fncSpeedRef('s',0,0);
tspan = 1:0.1:5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur

[R,K,d] = rigidityMatrixFnc(conRobot);
_zero = zeros(size(R(corRobot,K),1),1);
sInit = [corRobot; _zero;];
sAnsInit =[_zero; _zero;];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
printf("Mulai memecakan masalah \n\n");
startExe = tic;

dydt = @(t, y) systm_robot(t, y,(dRobot*length_d), R,K, kp, ki, B, fvref(t));
[t,y] = ode45(dydt, tspan, sInit);

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")

save DataOutMotion.data y
save DataErrorEdge.data yans