-
Notifications
You must be signed in to change notification settings - Fork 12
/
Simulator.m
80 lines (65 loc) · 2.79 KB
/
Simulator.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
function Simulator()
%% --------------------------------------------------%
% main simulation
% --------------------------------------------------- input
% --------------------------------------------------- output
% ---------------------------------------------------
%% -------------------------------------------------- set parameters
global config sensor form;
[config, sensor, form] = ConfigFile();
fig = figure(1); % for the main simulator
set(fig, 'position', [675 105 700 700]);
hold on;
% --------------------------------------------------% *movie*
if config.movie == 1 %
winsize = get(fig, 'Position'); %
winsize(1 : 2) = [0 0]; %
numframes = 440; %
A = moviein(numframes, fig, winsize); %
set(fig, 'NextPlot', 'replacechildren'); %
end %
% --------------------------------------------------%
%% -------------------------------------------------- initialize
t = 1;
% --------------------------------------------------% *movie*
if config.movie == 1 %
A(:, t) = getframe(fig, winsize); %
end %
% --------------------------------------------------%
global data obs;
[data, obs] = Initialize(t, config);
DrawWorld(data.p(t, :), data.u(t, :), obs, config, sensor);
grid on;
%% -------------------------------------------------- main loop
while t < config.T
if config.pause
disp('--------------------------');
pause;
else
pause(0.015);
end
t = t + 1;
% ----------------------------------------------% *movie*
if config.movie %
A(:, t) = getframe(fig, winsize); %
end %
% ----------------------------------------------%
clf;
[data, form] = Control(t, data, config, sensor, form);
data = Move(t, data, config);
data = Sense(t, data, obs, config, sensor);
DrawWorld(data.p(t, :), data.u(t, :), obs, config, sensor);
grid on;
end
%% -------------------------------------------------- save
save log.mat ...
config sensor form ...
data obs...
t
% --------------------------------------------------% *movie*
if config.movie %
save movie.mat A; %
movie2avi(A, 'movie.avi'); %
end %
% --------------------------------------------------%
end