74 lines
2.5 KiB
Matlab
74 lines
2.5 KiB
Matlab
function [t,y,Dd,DdNorm] = run_my_simulation(bParam)
|
|
algo = 0;
|
|
d = [[]];
|
|
ang = [];
|
|
save_state = [];
|
|
save algo_info.m algo d ang save_state;
|
|
|
|
robStateLen = size(bParam.model.origin.A,1);
|
|
|
|
% 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);
|
|
|
|
## sys = formationControl2ndBuilder(bParam);
|
|
% sys = formationControl1stBuilder(bParam);
|
|
sys = formationControl2ndModelBuilder(bParam);
|
|
% sys = formationControl1stModelBuilder(bParam);
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
|
|
|
|
_zero = zeros(size(sys.R(bParam.corRobot,sys.K),1),1);
|
|
sInit = [bParam.corRobot; _zero;];
|
|
s1stInit = [bParam.corRobot;
|
|
zeros(size(bParam.corRobot));
|
|
zeros(size(sys.d))];
|
|
s2ndInit = [bParam.corRobot;
|
|
zeros(length(bParam.corRobot),1);
|
|
zeros(length(bParam.corRobot),1);
|
|
_zero;];
|
|
s2ndModelInit = [bParam.corRobot;
|
|
zeros(length(bParam.corRobot),1);
|
|
zeros(length(bParam.corRobot),1);
|
|
_zero;
|
|
zeros(length(bParam.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, bParam.tspan, state_init);
|
|
|
|
% discrate taylor methode
|
|
## dydt = @(t, y) sys.dss(y, bparam.hStep, fvref(t));
|
|
## t(1,:) = bParam.tspan(1);
|
|
## y(1,:) = dydt(t(1), state_init);
|
|
## for i = 2:length(bParam.tspan)
|
|
## t(i,:) = bParam.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))]';
|
|
DdNorm(1) = norm(Dd(1,:));
|
|
for i = 2:length(bParam.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))]';
|
|
DdNorm(i) = norm(Dd(i,:));
|
|
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
|
|
endfunction |