forked from wangting0310njtech/Multi-robot-SLAM-Code
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathsimrobcb.m
More file actions
381 lines (330 loc) · 14.3 KB
/
simrobcb.m
File metadata and controls
381 lines (330 loc) · 14.3 KB
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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
function simrobcb(action)
% SIMROBCB this is callback function for context menu of robots
h = findobj('Tag','ListStore');
list = get(h,'UserData');
number = get(gcbo,'UserData'); % get robot's number from caller's UserData property
for i = 1:length(list); % Find position of the robot in the list
if getnum(list(i)) == number
pos = i;
break
end
end
% The MATRIX is modified by DLLs. These DLLs work with the pointer to the matrix
% so there is only one copy of matrix variable in the memory at a time, which is
% directly modified and it's not necessary to update it every time.
% But, it's there to make the program look 'correct'
switch(action)
case 'info'
% *********** Get data and display them ************
name = ['Name ' getnameR(list(pos))];
algfile = ['Algorithm ' getaf(list(pos))];
numstr = ['Number ' int2str(getnum(list(pos)))];
posstr = ['Position [' int2str(getpos(list(pos))) ']'];
headstr = ['Heading ' int2str(gethead(list(pos))) '°'];
scalestr = ['Scale ' int2str(getscale(list(pos)))];
powerstr = ['Power ' int2str(getpower(list(pos)))];
sensstr = ['Sensors ' int2str(length(getsens(list(pos))))];
infotext = strvcat(name,algfile,numstr,posstr,headstr,scalestr,powerstr,sensstr);
h = msgbox(infotext,'Info','help');
% **************************************************
case 'chscale'
scale = getscale(list(pos));
scale = inputdlg('Scale:','Scale',[1 15],{num2str(scale)});
if ~isempty(scale)
h = findobj('Tag','EditorWindow');
matrix = get(h,'UserData');
deleteR(list(pos))
list(pos) = setscale(list(pos), scale{1});
list(pos) = putrob(list(pos), getpos(list(pos)), matrix);
set(h,'UserData',matrix)
h = findobj('Tag','ListStore');
set(h,'UserData',list)
end
case 'chcol'
oldcolor = getcolor(list(pos));
color = uisetcolor(oldcolor,'Set Color');
list(pos) = setcolor(list(pos),color);
h = findobj('Tag','ListStore');
set(h,'UserData',list);
case 'chshape'
oldsh = getshape(list(pos));
shape = inputdlg({'X data (default shape - [-2 0 4 0 -2])',...
'Y data (default shape - [3 4 0 -4 -3])'},...
'Enter data for new shape',[1 45],...
{num2str(oldsh.xdata),num2str(oldsh.ydata)});
if ~isempty(shape)
xdata = str2num(shape{1});
ydata = str2num(shape{2});
h = findobj('Tag','EditorWindow');
matrix = get(h,'UserData');
deleteR(list(pos))
list(pos) = chshape(list(pos),xdata,ydata);
list(pos) = putrob(list(pos), getpos(list(pos)), matrix);
set(h,'UserData',matrix)
h = findobj('Tag','ListStore');
set(h,'UserData',list)
end
case 'name'
name = inputdlg('Name:','Name',[1 15],{getnameR(list(pos))});
if ~isempty(name)
list(pos) = setname(list(pos), name{1});
h = findobj('Tag','ListStore');
set(h,'UserData',list)
end
case 'power'
gui_power(getpower(list(pos)), number);
case 'power_ok'
h = findobj('Tag','PowerMenu');
power = get(h,'Value');
if power == 1
list(pos) = setpower(list(pos),1);
else
list(pos) = setpower(list(pos),0);
end
h = findobj('Tag','ListStore');
set(h,'UserData',list);
close(gcbf);
case 'delete'
answer = questdlg('Really delete this robot?','Question','Yes','No','No');
if strcmp(answer,'Yes')
h = findobj('Tag','EditorWindow');
matrix = get(h,'UserData');
deleteR(list(pos));
list(pos) = [];
for i = pos:length(list)
list(i) = setnum(list(i),i + 1);
deleteR(list(i))
list(i) = putrob(list(i),getpos(list(i)),matrix);
end
h = findobj('Tag','ListStore');
set(h,'UserData',list);
if isempty(list)
h = findobj('Tag','SaveMenu');
set(h,'Enable','off')
end
end
case 'clearmem'
answer = questdlg('Really clear memory of this robot?','Question','Yes','No','No');
if strcmp(answer,'Yes')
list(pos) = writemem(list(pos),[]);
h = findobj('Tag','ListStore');
set(h,'UserData',list);
end
case 'showmem'
mem = readmem(list(pos));
contents = whos('mem');
if ~isempty(mem)
dispstr = strvcat(['Size: ' num2str(contents.size)],...
['Bytes: ' num2str(contents.bytes)],['Class: ' contents.class]);
else
dispstr = 'No contents - empty memory';
end
h = msgbox(dispstr,'Memory contents','help');
case 'savemem'
mem = readmem(list(pos));
if ~isempty(mem)
[filename,pathname] = uiputfile('memory.mat','Save memory contents');
if filename ~= 0
save([pathname filename],'mem');
end
else
h = msgbox('The memory is empty','Memory contents','help');
end
case 'saveasc'
mem = readmem(list(pos));
if ~isempty(mem)
if ~isa(mem,'double')
h = msgbox('Only numeric matrices can be saved as ASCII data files','Warning','warn');
else
[filename,pathname] = uiputfile('memory.asc','Save memory contents');
if filename ~= 0
save([pathname filename],'mem','-ascii','-double','-tabs');
end
end
else
h = msgbox('The memory is empty','Memory contents','help');
end
case 'savemat'
mem = readmem(list(pos));
if ~isempty(mem)
[filename,pathname] = uiputfile('memory.mat','Save memory contents');
if filename ~= 0
save([pathname filename],'mem');
end
else
h = msgbox('The memory is empty','Memory contents','help');
end
case 'loadmat'
mem = readmem(list(pos));
if ~isempty(mem)
answer = questdlg('Overwrite memory contents?','Question','Yes','No','No');
else
answer = 'Yes';
end
if strcmp(answer,'Yes')
[filename,pathname] = uigetfile('memory.mat','Upload mat file into memory');
if filename ~= 0
vars = who('-file',[pathname filename]); % Lists variables in selected file
% ** Try to find 'mem' in variable list **
cmp = strcmp(vars,'mem');
if ~any(cmp)
h = msgbox('Could not find variable called ''mem'' in selected file. Only variable ''mem'' can be loaded into memory.','Error','error');
else
% ****************************************
load([pathname filename]);
list(pos) = writemem(list(pos),mem);
h = findobj('Tag','ListStore');
set(h,'UserData',list)
end
end
end
case 'loadasc'
mem = readmem(list(pos));
if ~isempty(mem)
answer = questdlg('Overwrite memory contents?','Question','Yes','No','No');
else
answer = 'Yes';
end
if strcmp(answer,'Yes')
[filename,pathname] = uigetfile('memory.asc','Upload ascii file into memory');
if filename ~= 0
try
mem = load([pathname filename],'-ascii');
catch
h = msgbox('Error loading file (wrong structure?)!','Error','error');
end
list(pos) = writemem(list(pos),mem);
h = findobj('Tag','ListStore');
set(h,'UserData',list)
end
end
case 'move'
load cur_put % Load cursor shape data
set(gcf,'Pointer','custom',...
'PointerShapeCData',cdata,... % User data defining pointer shape
'PointerShapeHotSpot',[11 5]); % Pointer active point (center of the p.)
h = findobj('Tag','Axes'); % Main window axes
set(h,'ButtonDownFcn','simrobcb move_click') % Set callback fcn & wait for a click
set(h,'UserData',getnum(list(pos))) % Axes will be callback caller of simrobcb
case 'move_click' % Click after move command
% ********* Get coordinates of mouse click *********
cpoint = get(gca,'CurrentPoint');
cpoint = round(cpoint(1,1:2));
% **************************************************
% ********* Change cursor back to arrow ************
h = findobj('Tag','Axes'); % Main window axes
set(h,'ButtonDownFcn','') % Delete callback
set(gcf,'Pointer','arrow',...
'PointerShapeHotSpot',[1 1])
% **************************************************
% ********** Make moved robot disappear ************
deleteR(list(pos))
% **************************************************
% ********** Get matrix and put in the robot *******
h = findobj('Tag','EditorWindow');
matrix = get(h,'UserData');
list(pos) = delhist(list(pos));
list(pos) = putrob(list(pos), cpoint, matrix);
% **************************************************
% ********* Store updated list ***********
h = findobj('Tag','ListStore');
set(h,'UserData',list)
% ****************************************
case 'heading'
gui_head(round(gethead(list(pos))));
h = findobj('Tag','OkButton');
set(h,'UserData',getnum(list(pos)))
case 'head_ok'
h = findobj('Tag','HeadSlider');
heading = get(h,'Value');
heading = round(heading);
h = findobj('Tag','EditorWindow');
matrix = get(h,'UserData');
list(pos) = delhist(list(pos));
list(pos) = sethead(list(pos), heading);
deleteR(list(pos))
close(gcbf) % Close the dialog
list(pos) = putrob(list(pos),getpos(list(pos)),matrix);
h = findobj('Tag','ListStore');
set(h,'UserData',list);
case 'sensors'
gui_sens;
h = findobj('Tag','SensStore');
set(h,'UserData',getsens(list(pos)))
h = findobj('Tag','SensAxes');
scale = getscale(list(pos));
set(h,'UserData',scale);
shape = getsshape(list(pos));
h = findobj('Tag','SensOK');
set(h,'Callback','simrobcb sens_ok','UserData',getnum(list(pos)))
h = findobj('Tag','SensCancel');
set(h,'UserData',shape)
gui_senscb initialize;
case 'sens_ok'
h = findobj('Tag','SensStore');
sensors = get(h,'UserData');
for i=1:length(sensors)
sensors(i).name = strcat(sensors(i).name,'');
end
list(pos) = addsenss(list(pos),sensors);
h = findobj('Tag','ListStore');
set(h,'UserData',list);
close(gcbf);
case 'algedit'
notepath = 'notepad';
fullpath = which(getaf(list(pos)));
conflict = 0;
answer = 'Yes';
if ~strcmp(fullpath,'..');
i = length(list);
for j = 1:i
algpath = which(getaf(list(j)));
if strcmp(algpath,fullpath) & j ~= pos
conflict = 1;
break
end
end
if conflict
answer = questdlg('Warning: This algorithm is also used for other robot(s). Edit anyway?','Warning','Yes','No','No');
end
if strcmp(answer,'Yes')
% cmd = ['! ' notepath ' ' fullpath];
% open algorithm in notepad (not possible in MATLAB editor)
% eval(cmd);
open(fullpath);
getaf(list(pos))
end
else h = msgbox('Error: File not found','Error occurred','error');end
case 'algnew'
notepath = 'notepad';
answer = questdlg('Create new algorithm?','Question','Yes','No','No');
if strcmp(answer,'Yes')
fullpath = which('algtemp.m'); % path to algorithm template
[path name ext] = fileparts(fullpath);
cmd = ['cd ' path];
% eval(cmd);
[newalg, newpath] = uiputfile('*.m','Create new file');
list(pos) = setaf(list(pos),newalg(1:length(newalg)-2));
newalg = [newalg]; % add extension
fnewpath = fullfile(newpath,newalg); % create path to new alg. file from path & filename
result = copyfile(fullpath,fnewpath); % copy template and then open it as new file
if result == 0
h = msgbox('Error: Error creating file','Error occurred','error');
else
% cmd = ['! ' notepath ' ' fnewpath]
% open algorithm in notepad (not possible in MATLAB editor)
% eval(cmd);
open(fnewpath);
end
end
h = findobj('Tag','ListStore');
set(h,'UserData',list);
case 'algassign'
[filename,pathname] = uigetfile('*.m','Assign algorithm file');
if filename ~= 0 % if any file selected
filename = strrep(filename,'.m',''); % delete extension .m
list(pos) = setaf(list(pos),filename);
end
h = findobj('Tag','ListStore');
set(h,'UserData',list);
end