diff --git a/SOURCE/RP_2015_Rozenheck.tex b/SOURCE/RP_2015_Rozenheck.tex index f905066..b689156 100644 --- a/SOURCE/RP_2015_Rozenheck.tex +++ b/SOURCE/RP_2015_Rozenheck.tex @@ -69,8 +69,10 @@ refrensi. $e = \begin{bmatrix} e_1^T \\ \vdots \\ e_m^T \end{bmatrix} \in \mathbb{R}^{2m}$ & Edge Vector \\ $E \in R^{n\times m}$ & Incidence matrix dimana isinya adalah $\{0, \pm 1\}$ \\ - & barisnya menandakan vertices dan kolom nya menandakan edge \\ \hline - \hline + & barisnya menandakan vertices dan kolom nya menandakan edge \\ \hline + $A_1, \dots, A_i \in \mathbb{R}^{p \times q}$ & \\ + $diag(A_i) \triangleq blkdiag\{A_1, \dots, A_n\} \in \mathbb{R}^{np \times nq}$ & https://www.mathworks.com/help/matlab/ref/blkdiag.html \\ + \hline \end{tabular} %% \end{table} @@ -182,7 +184,7 @@ Dengan menerapkan kendali PI Menghasilkan state baru dan Ditulis ulang \begin{align} \dot{x}_1 =& x_{2}(t)\\ - \dot{x}_2 =& -k_{p1}x_2(t) -R(x_1)^T k_{p2}(R(x_1)x_1(t) - d )) - \xi_1 - R(x_1) \xi_2 \\ + \dot{x}_2 =& -k_{p1}x_2(t) -R(x_1)^T k_{p2}(R(x_1)x_1(t) - d )) - \xi_1 - R(x_1)^T \xi_2 \\ \dot{\xi}_1 =& k_{i1} x_2(t)\\ \dot{\xi}_2 =& k_{i2} (R(x_1)x_1(t) - d) \end{align} @@ -193,7 +195,7 @@ Dalam bentuk matrix \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0\\ - -k_{p2}R(x_1)^T R(x_1) & -k_{p1} & -1 & -R(x_1) \\ + -k_{p2}R(x_1)^T R(x_1) & -k_{p1} & -1 & -R(x_1)^T \\ 0 & k_{i1} & 0 & 0 \\ k_{i2}R(x_1) & 0 & 0 & 0 \\ \end{bmatrix} diff --git a/SOURCE/SM_2015_Rozenheck.m b/SOURCE/SM_2015_Rozenheck.m index 1fe771b..0f0b951 100644 --- a/SOURCE/SM_2015_Rozenheck.m +++ b/SOURCE/SM_2015_Rozenheck.m @@ -1,85 +1,48 @@ 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; + 3 2 1; + 3 1 1; ]; -length_d = 3; +length_d = .5; 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 + ]; % 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); +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:5; +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_robot(t, y,(dRobot*length_d), R,K, kp, ki, B, fvref(t)); -[t,y] = ode45(dydt, tspan, sInit); - +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++; @@ -133,7 +96,7 @@ function plot_con (pltRb, yOut, conIn,xm,ym, time) 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, round(length(tspan)/2)); plot_con(plot_rb, y', conRobot, xrb, yrb, length(tspan)); @@ -171,6 +134,7 @@ 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 +csvwrite("DataOutMotion.csv", [t' y']) +% save DataOutMotion.data y +% save DataErrorEdge.data yans diff --git a/SOURCE/rigidityMatrixFnc.m b/SOURCE/rigidityMatrixFnc.m index 1cbc1e4..72270f5 100644 --- a/SOURCE/rigidityMatrixFnc.m +++ b/SOURCE/rigidityMatrixFnc.m @@ -22,7 +22,7 @@ function [R,K, d] = rigidityMatrixFnc (edgeL) endfor str = strcat(str,sprintf("errVec(%i:%i)' ));",i+2,i+3)); % return sebagai fungsi R - eval(str); + eval(str); %% replicate from https://www.mathworks.com/help/matlab/ref/blkdiag.html R = @(x,k) errBlockDiagonal(edgeVector(x,k))*k; endfunction diff --git a/SOURCE/systm_2nd_order_robot.m b/SOURCE/systm_2nd_order_robot.m new file mode 100644 index 0000000..a62c67a --- /dev/null +++ b/SOURCE/systm_2nd_order_robot.m @@ -0,0 +1,32 @@ + +%% systm_2nd_robot +%% param : +%% t -> time +%% x -> state +%% d -> distance antara robot nya +%% R, K -> return dari fungsi rigidityMatrixFnc() +%% kp -> konstanta proporsional +%% ki -> konstanta integral +%% B -> matrix selctor vref +%% vref -> kecepatan refrensi +function dxdt = systm_2nd_order_robot(t,x,d,R,K,kp1,kp2,ki1,ki2,B,vref) + n = length(K(1,:))/2; + l = length(R(ones(n*2,1),K)); + m = length(K(:,1))/2; + dxdt = zeros(length(x),1); + x1 = x(1:n*2); + len = length(x1); + x2 = x(len+1:len+length(x1)); + len = len + length(x2); + x3 = x(len+1:len+length(x2)); + len = len + length(x3); + x4 = x(len+1:length(x)); + + dxdt(1:n*2) = x2; + len = length(x1); + dxdt(len+1:len+length(x1)) = -kp1*x2 -kp2*R(x1,K)'*R(x1,K)*x1 +kp2*R(x1,K)'*d -x3 -R(x1,K)'*x4 + (B*vref); + len = len + length(x2); + dxdt(len+1:len+length(x2)) = ki1*x2; + len = len + length(x3); + dxdt(len+1:length(x)) = ki2*R(x1,K)*x1 -ki2*d; +endfunction diff --git a/SOURCE/systm_robot.m b/SOURCE/systm_robot.m index 1174f19..35423d1 100644 --- a/SOURCE/systm_robot.m +++ b/SOURCE/systm_robot.m @@ -1,3 +1,13 @@ +%% systm_robot +%% param : +%% t -> time +%% x -> state +%% d -> distance antara robot nya +%% R, K -> return dari fungsi rigidityMatrixFnc() +%% kp -> konstanta proporsional +%% ki -> konstanta integral +%% B -> matrix selctor vref +%% vref -> kecepatan refrensi function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref) n = length(K(1,:))/2; l = length(R(ones(n*2,1),K)); @@ -5,14 +15,6 @@ function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref) dxdt = zeros(l,1); x1 = x(1:n*2); x2 = x(length(x1)+1:length(x)); - %% if (t > 5) && (t < 15) - %% vref = [5; 10]; - %% else - %% if (t > 15) && (t > 20) - %% vref = [5; -10]; - %% endif - %% endif - %% dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref); dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref); dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*d); endfunction