-
Notifications
You must be signed in to change notification settings - Fork 0
/
CreateRobot.m
3570 lines (3193 loc) · 153 KB
/
CreateRobot.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
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
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
classdef CreateRobot < handle
% Creates definition of class, instances of which will include all
% properties and controls associated with a single CreateRobot robot
% CreateRobot.m
% Copyright (C) 2011 Cornell University
% This code is released under the open-source BSD license. A copy of this
% license should be provided with the software. If not, email:
properties(Constant, GetAccess= 'private')
% Values cannot be changed
% All units are kg/m/s/rad unless otherwise noted
radius= 0.2; % Radius of the robot (model as a circle)
rangeIR= 0.1; % Linear range of the infrared sensor
rangeSonar= 3; % Linear range of all sonar sensors
rangeMinSonar= 0.02;% Minimum linear range of sonar sensor
rangeLidar= 4; % Linear range of LIDAR sensor
rangeMinLidar= 0.02;% Minimum linear range of LIDAR sensor
angRangeLidar= pi*240/180; % Angular range of LIDAR sensor
numPtsLidar= 681; % Number of points in LIDAR sensing range
rangeCamera= 6; % Linear range of the camera for blob detection
angRangeCamera= pi*60/180; % Angular range of camera
frictionKin= 0.35; % Coefficient of kinetic friction of robot and wall
end
properties(GetAccess= 'private', SetAccess= 'private')
% Values require get and set methods to view or manipulate
% Sensor variables
odomDist; % Distance traveled since last check from odometry
% Format: double
odomAng; % Angle turned since last check from odometry
% Format: double
noise; % Contains noise data for sensors used
% Format: structure, fieldnames are sensor names and
% field values are sensor noise [mean standard_dev]
comDelay; % Communication delay of commands to robot
% Format: double
% Robot state variables
posAbs; % Position in absolute coordinates
% Format: vector of doubles, [x y]
velAbs; % Velocity in absolute coordinates
% Format: vector of doubles, [x y]
thAbs; % Yaw angle relative to positive x-axis
% Format: double (-pi < thAbs <= pi)
wAbs; % Angular velocity
% Format: double (positive counter-clockwise)
velInt; % Forward velocity intended by motor commands
% Format: double
wInt; % Angular velocity intended by motor commands
% Format: double
autoEnable; % Enabled/Disabled state of autonomous control
% Format: boolean
dataHist; % Time history of position and function calls for output
% Format: cell array, columns
% [time x y th fcn_called arg_val]
% time - Double, time relative to start of autonomous code
% x - Double, x-coordinate of center of robot
% y - Double, y-coordinate of center of robot
% th - Double, angle of robot relative to pos x-axis
% fcn_called - String or cell array of strings,
% name(s) of the function(s) called in that time step
% arg_val - Vector of doubles or cell array of vectors,
% value(s) for the input OR output argument(s);
% the robot object argument is ignored
% Environment variables
timeElap; % Time of the start of autonomous code execution
% Format: unsigned 64 bit integer
% time syntax - to be used with toc function
handlesGUI; % Handles to all GUI objects
% Format: structure containing handle numbers
% See SimulatorGUI.fig and SimulatorGUI.m for more
% format information on the contents of the structure
mapStart; % Contains robot start position/orientation information
% Format: vector of doubles, [x y th]
mapWalls; % Contains obstacle (wall) start and end points
% Format: matrix of doubles, columns [x1 y1 x2 y2]
mapLines; % Contains line start and endpoints
% Format: matrix of doubles, columns [x1 y1 x2 y2]
mapBeacs; % Containing beacon location, color, and ID information
% Format: cell array of doubles,
% columns {x y red green blue ID}
mapVWalls; % Contains virtual wall location, direction, and
% strength information
% Format: matrix of doubles, columns [x y th strength]
end
methods(Static, Access= 'public')
% Functions that do not need the object defined to operate
function newAng= wrap2pi(oldAng)
% newAng = wrap2pi(oldAng)
% Wrap angle in radians to [-pi pi]
% Replace wrapToPi to avoid dependence on mapping toolbox
%
% Input:
% oldAng - Angle to be wrapped within limits (rad)
%
% Output:
% newAng - Output angle within limits (rad)
% Increase if to low
while oldAng < -pi
oldAng= oldAng+2*pi;
end
% Decrease if too high
while oldAng > pi
oldAng= oldAng-2*pi;
end
newAng= oldAng;
end
end
methods(Access= 'public')
% Functions available to call from controller function or other files
% Constructor Function
function obj= CreateRobot(varargin)
% obj = CreateRobot
% Creates instance of the user-defined class Create Robot and
% initializes all properties. Note that if CreateRobot is not
% called by SimulatorGUI (with handlesGUI argument), full
% functionality impossible.
%
% obj = CreateRobot(handlesGUI)
% Format of function call from SimulatorGUI. Passes along structure
% of handles for the GUI to allow full functionality
%
% Input:
% handlesGUI - Structure of handles to GUI objects
% e.g. handlesGUI.push_adv - handle for advance button
%
% Output:
% obj - Instance of class CreateRobot with all fields initialized
% Deal with input argument
% Will be handles to SimulatorGUI if called by simulator
if ~isempty(varargin) && isstruct(varargin{1})
obj.handlesGUI= varargin{1};
else
obj.handlesGUI= [];
end
% Assign properties
obj.mapStart= [0 0 0]; % Default start position at origin
obj.mapWalls= [];
obj.mapLines= [];
obj.mapBeacs= {};
obj.mapVWalls= [];
obj.timeElap= [];
obj.dataHist= {};
obj.noise= struct; % Empty structure to store noise data
obj.comDelay= 0; % Default instantaneous communication
obj.posAbs= obj.mapStart(1:2); % Initial position
obj.velAbs= [0 0]; % Assume robot is stationary to start
obj.thAbs= obj.mapStart(3);
obj.wAbs= 0;
obj.velInt= 0;
obj.wInt= 0;
obj.autoEnable= false; % Start in manual control mode
obj.odomDist= 0; % Start odometry at zero
obj.odomAng= 0;
end
% SimulatorGUI Control Functions
function [rad rIR rSon rLid angRLid numPtsLid]= getConstants(obj)
% [rad rIR rSon rLid angRLid numPtsLid] = getConstants(obj)
% Output constant properties of the robot for simulator usage
%
% Input:
% obj - Instance of class CreateRobot
%
% Output:
% rad - Double, radius of robot (m)
% rIR - Double, range of infrared wall sensor (m)
% rLid - Double, linear range of LIDAR sensor (m)
% angRLid - Double, angular range of LIDAR sensor (rad)
% numPtsLid - Double, number of points used by LIDAR sensor
rad= obj.radius;
rIR= obj.rangeIR;
rSon= obj.rangeSonar;
rLid= obj.rangeLidar;
angRLid= obj.angRangeLidar;
numPtsLid= obj.numPtsLidar;
end
function [start walls lines beacs vwalls]= getMap(obj)
% [start walls lines beacs vwalls] = getMap(obj)
% Output the obstacle and robot map data for plotting
%
% Input:
% obj - Instance of class CreateRobot
%
% Output:
% start - Vector containing robot start position information
% walls - Matrix containing obstacle information
% lines - Matrix containing line information
% beacs - Cell array containing beacon information
% vwalls - Matrix containing virtual wall information
% See the properties specification for format information
% Assign variables from object values
start= obj.mapStart;
walls= obj.mapWalls;
lines= obj.mapLines;
beacs= obj.mapBeacs;
vwalls= obj.mapVWalls;
end
function setMap(obj,walls,lines,beacs,vwalls)
% setMap(obj,walls,lines,beacs,vwalls)
% Change the obstacle map data in the robot object
%
% Input:
% obj - Instance of class CreateRobot
% walls - % Matrix containing obstacle information
% lines - % Matrix containing line information
% beacs - % Cell array containing beacon information
% vwalls - % Matrix containing virtual wall information
% See properties specification for format information
% Save map information to object
obj.mapWalls= walls;
obj.mapLines= lines;
obj.mapBeacs= beacs;
obj.mapVWalls= vwalls;
end
function setMapStart(obj,origin)
% setMapStart(obj,origin)
% Change the mapStart data in the robot object. Move the robot to
% that position.
%
% Input:
% obj - Instance of class CreateRobot
% origin - Vector of doubles, containing information about the
% robot start position in the format [x y th]
obj.mapStart= origin;
obj.posAbs= origin(1:2);
obj.thAbs= origin(3);
end
function setNoise(obj,noiseStruct)
% setNoise(obj,noiseStruct)
% Change the noise data for the sensors in the robot object
%
% Input:
% obj - Instance of class CreateRobot
% noise - Structure, containing noise information
% Fieldnames are sensor names with values of [mean standard_dev]
% eg. noise.lidar = [0.003 0.012]
% Set property value
obj.noise= noiseStruct;
end
function setComDelay(obj,tDelay)
% setComDelay(obj,tDelay)
% Change the communication delay property in the robot object
% This delay will occur on every function call from the
% autonomous program
%
% Input:
% obj - Instance of class CreateRobot
% tDelay - Double, communication delay (s)
obj.comDelay= tDelay;
end
function manualKeyboard(obj,velChange)
% manualKeyboard(obj,velChange)
% Change the velocity of the robot according to manual controls
%
% Input:
% obj - Instance of class CreateRobot
% velChange - Vector of doubles [linear angular], controlling
% velocity; positive increases, negative decreases, zero leaves
% it as is, and NaN will zero the velocity
% Change robot object property values
if any(isnan(velChange))
obj.velInt= 0;
obj.wInt= 0;
else
% Find intended velocity
FwdVel= obj.velInt+velChange(1);
AngVel= obj.wInt+velChange(2);
% Limit individual wheel velocities
wheelRight= FwdVel+AngVel*obj.radius;
wheelLeft= FwdVel-AngVel*obj.radius;
if abs(wheelRight) > 0.5 || abs(wheelLeft) > 0.5
wheelRight= min(max(wheelRight,-0.5),0.5);
wheelLeft= min(max(wheelRight,-0.5),0.5);
disp(['Warning: desired velocity combination '...
'exceeds limits'])
FwdVel= (wheelRight+wheelLeft)/2;
AngVel= (wheelRight-wheelLeft)/(2*obj.radius);
end
obj.velInt= FwdVel;
obj.wInt= AngVel;
end
end
function setAutoEnable(obj,state)
% setAutoEnable(obj,state)
% Imports new state for autoEnable property
%
% Input:
% obj - Instance of class CreateRobot
% state - Boolean, true if autonomous control is enabled
obj.autoEnable= state;
end
function autoCheck(obj)
% autoCheck(obj)
% Checks if autonomous control program is allowed to execute
% If it is not allowed to execute, it will throw an error to exit
% all currently running functions until the error is caught
%
% Input:
% obj - Instance of class CreateRobot
% Check if autonomous control is disabled
if ~obj.autoEnable
% Throw exception to exit autonomous control program
error('SIMULATOR:AutonomousDisabled',...
['Autonomous code execution is disabled. '...
'SimulatorGUI must be open and it must call the '...
'control program for autonomous mode to enable'])
end
end
function updateSensorVisualization(obj,handlesGUI,handles_sensors)
% updateSensorVisualization(obj,handlesGUI,handles_sensors)
% Updates the data used to plot the sensors visual representation
%
% Input:
% obj - Instance of class CreateRobot
% handlesGUI - Structure of doubles, handles to all GUI objects
% handles_sensors - Vector of doubles, handles to all sensor
% visualization plots;
% order specified by creation in SimulatorGUI_OpeningFcn
% Wall sensor
if get(handlesGUI.chkbx_wall,'Value')
% Calculate position of sensor
sensAng= -0.225*pi; % Sensor placement relative to front
x_sensor= obj.posAbs(1)+obj.radius*cos(obj.thAbs+sensAng);
y_sensor= obj.posAbs(2)+obj.radius*sin(obj.thAbs+sensAng);
sensAngDir= -0.5*pi; % Sensor looks to the right
% Find if wall is detected
wall= genIR(obj);
if wall % Plot full range if wall is detected
set(handles_sensors(1),'XData',[x_sensor x_sensor+...
obj.rangeIR*cos(obj.thAbs+sensAngDir)])
set(handles_sensors(1),'YData',[y_sensor y_sensor+...
obj.rangeIR*sin(obj.thAbs+sensAngDir)])
set(handles_sensors(1),'Visible','on')
else
set(handles_sensors(1),'Visible','off')
end
end
% Sonar sensors
if get(handlesGUI.chkbx_sonar,'Value')
distSonar= genSonar(obj);
for i= 1:4
th_sensor= obj.thAbs+(i-1)*pi/2;
x_sensor= obj.posAbs(1)+obj.radius*cos(th_sensor);
y_sensor= obj.posAbs(2)+obj.radius*sin(th_sensor);
set(handles_sensors(1+i),'XData',...
[x_sensor x_sensor+distSonar(i)*cos(th_sensor)])
set(handles_sensors(1+i),'YData',...
[y_sensor y_sensor+distSonar(i)*sin(th_sensor)])
end
end
% Bump sensors
if get(handlesGUI.chkbx_bump,'Value')
bump= genBump(obj);
x_robot= obj.posAbs(1);
y_robot= obj.posAbs(2);
th_robot= obj.thAbs;
if bump(1) % Right bump sensor
set(handles_sensors(6),'XData',...
[x_robot+obj.radius*cos(th_robot-pi/2) ...
x_robot+obj.radius*cos(th_robot-0.135*pi)])
set(handles_sensors(6),'YData',[y_robot+obj.radius*...
sin(th_robot-pi/2) y_robot+obj.radius*...
sin(th_robot-0.135*pi)])
set(handles_sensors(6),'Visible','on')
else
set(handles_sensors(6),'Visible','off')
end
if bump(2) % Front bump sensor
set(handles_sensors(7),'XData',...
[x_robot+obj.radius*cos(th_robot-0.135*pi) ...
x_robot+obj.radius*cos(th_robot+0.135*pi)])
set(handles_sensors(7),'YData',...
[y_robot+obj.radius*sin(th_robot-0.135*pi) ...
y_robot+obj.radius*sin(th_robot+0.135*pi)])
set(handles_sensors(7),'Visible','on')
else
set(handles_sensors(7),'Visible','off')
end
if bump(3) % Left bump sensor
set(handles_sensors(8),'XData',...
[x_robot+obj.radius*cos(th_robot+0.135*pi) ...
x_robot+obj.radius*cos(th_robot+pi/2)])
set(handles_sensors(8),'YData',...
[y_robot+obj.radius*sin(th_robot+0.135*pi) ...
y_robot+obj.radius*sin(th_robot+pi/2)])
set(handles_sensors(8),'Visible','on')
else
set(handles_sensors(8),'Visible','off')
end
end
% Cliff sensors
if get(handlesGUI.chkbx_cliff,'Value')
cliff= genCliff(obj);
x_robot= obj.posAbs(1);
y_robot= obj.posAbs(2);
th_robot= obj.thAbs;
if cliff(1) <= 5.4 % Right cliff sensor
set(handles_sensors(9),'XData',...
x_robot+obj.radius*cos(th_robot-pi/3))
set(handles_sensors(9),'YData',...
y_robot+obj.radius*sin(th_robot-pi/3))
set(handles_sensors(9),'Visible','on')
else
set(handles_sensors(9),'Visible','off')
end
if cliff(2) <= 5.4 % Front-right cliff sensor
set(handles_sensors(10),'XData',...
x_robot+obj.radius*cos(th_robot-pi/10))
set(handles_sensors(10),'YData',...
y_robot+obj.radius*sin(th_robot-pi/10))
set(handles_sensors(10),'Visible','on')
else
set(handles_sensors(10),'Visible','off')
end
if cliff(3) <= 5.4 % Front-left cliff sensor
set(handles_sensors(11),'XData',...
x_robot+obj.radius*cos(th_robot+pi/10))
set(handles_sensors(11),'YData',...
y_robot+obj.radius*sin(th_robot+pi/10))
set(handles_sensors(11),'Visible','on')
else
set(handles_sensors(11),'Visible','off')
end
if cliff(4) <= 5.4 % Left cliff sensor
set(handles_sensors(12),'XData',...
x_robot+obj.radius*cos(th_robot+pi/3))
set(handles_sensors(12),'YData',...
y_robot+obj.radius*sin(th_robot+pi/3))
set(handles_sensors(12),'Visible','on')
else
set(handles_sensors(12),'Visible','off')
end
end
% LIDAR sensor
if get(handlesGUI.chkbx_lidar,'Value')
x_sensor= obj.posAbs(1)+obj.radius*cos(obj.thAbs);
y_sensor= obj.posAbs(2)+obj.radius*sin(obj.thAbs);
% Get noise parameters
if isfield(obj.noise,'lidar')
noiseAvg= obj.noise.lidar(1);
noiseStDev= obj.noise.lidar(2);
else
noiseAvg= 0;
noiseStDev= 0;
end
% % First line
% th_sensor= obj.thAbs-obj.angRangeLidar/2;
% dist= findDist(obj,x_sensor,y_sensor,...
% obj.rangeLidar,th_sensor)+noiseAvg+noiseStDev*randn;
% set(handles_sensors(13),'XData',...
% [x_sensor x_sensor+dist*cos(th_sensor)])
% set(handles_sensors(13),'YData',...
% [y_sensor y_sensor+dist*sin(th_sensor)])
%
% % Second line
% th_sensor= obj.thAbs+obj.angRangeLidar/2;
% dist= findDist(obj,x_sensor,y_sensor,...
% obj.rangeLidar,th_sensor)+noiseAvg+noiseStDev*randn;
% set(handles_sensors(14),'XData',...
% [x_sensor x_sensor+dist*cos(th_sensor)])
% set(handles_sensors(14),'YData',...
% [y_sensor y_sensor+dist*sin(th_sensor)])
% Sensor lines in LIDAR range
for i= 0:4
th_sensor= obj.thAbs+obj.angRangeLidar*(i-2)/4;
dist= findDist(obj,x_sensor,y_sensor,obj.rangeLidar,...
th_sensor)+noiseAvg+noiseStDev*randn;
set(handles_sensors(13+i),'XData',...
[x_sensor x_sensor+dist*cos(th_sensor)])
set(handles_sensors(13+i),'YData',...
[y_sensor y_sensor+dist*sin(th_sensor)])
end
end
end
function startTimeElap(obj)
% startTimeElap(obj)
% Start the timer to determine time steps for data output
%
% Input:
% obj - Instance of class CreateRobot
obj.timeElap= tic;
end
function updateOutput(obj)
% updateOutput(obj)
% Update the data history to be outputted after autonomous code
% execution is done
%
% Input:
% obj - Instance of class CreateRobot
% Add in state variable information, leave function empty
if obj.autoEnable % Autonomous mode
obj.dataHist= [obj.dataHist ; {toc(obj.timeElap)} ...
{obj.posAbs(1)} {obj.posAbs(2)} {obj.thAbs} {[]}];
end
end
function addFcnToOutput(obj,fcn_called)
% addFcnToOutput(obj,fcn_called)
% Add the string representing a function call to the output data in
% the current index
%
% Input:
% obj - Instance of class CreateRobot
% fcn_called - String representation of the function call including
% all arguments in the forms of (e.g.):
% RoombaInit - zero argument functions
% [1 0]= ButtonReaderRoomba - output argument functions
% travelDist(0.200,1.500) - input argument functions
incell= obj.dataHist{end,5}; % In function name cell
if isempty(incell) % No functions called yet
obj.dataHist{end,5}= fcn_called;
else % Function(s) have been called
if ischar(incell) % Single string
obj.dataHist{end,5}= ...
{incell ; fcn_called}; % Create cell array
else % Cell array of strings
incell{end+1}= fcn_called; % Add to cell array
obj.dataHist{end,5}= incell;
end
end
end
function saveOutput(obj)
% saveOutput(obj)
% Save the output data to a .mat file in the current directory
%
% Input:
% obj - Instance of class CreateRobot
% Extract autonomous data immediately
datahistory= obj.dataHist;
% Set default filename to unused file
filename= 'SimulatorOutputData_';
i= 1;
while exist([filename num2str(i) '.mat'],'file')
i= i+1; % Increment number to assign after filename
end
filename= [filename num2str(i)];
% Pull up dialogue box for saving file with default name
filename= inputdlg('Filename (no extension):',...
'Save Autonomous Data',1,{filename});
if ~isempty(filename)
filename= [filename{1} '.mat'];
% Check if file is already in existence
button= 'Yes';
if exist(filename,'file')
button= questdlg('A file by that name exists. Overwrite?',...
'Verify Overwrite','Yes','Cancel','Cancel');
end
% Note that 'Cancel' will not allow file to be saved
% Save file if desired
if strcmp(button,'Yes')
save(filename,'datahistory')
end
end
end
function resetOutput(obj)
% resetOutput(obj)
% Reset the data to be outputted after autonomous execution
%
% Input:
% obj - Instance of class CreateRobot
% Clear the robot object property
obj.dataHist= {};
end
% Sensor Functions
function bump= genBump(obj)
% bump = genBump(obj)
% Generates a reading for all bump sensors
% Input:
% obj - Instance of class CreateRobot
%
% Output:
% bump - Vector of Boolean doubles [right front left],
% 1 indicates the sensor is activated, 0 is not activated
% Get intersections with walls
collPts= findCollisions(obj);
% Check that there are intersections
bump= [0 0 0]; % Initialize to no bumps
if ~isempty(collPts)
% Change intersection points to local coordinates
x_int= collPts(:,1)-obj.posAbs(1);
y_int= collPts(:,2)-obj.posAbs(2);
% Get angle relative to robot front
th_int= obj.wrap2pi(atan2(y_int,x_int)-obj.thAbs);
% Find sensor activated, if any
for i= 1:length(th_int)
if th_int(i) < -0.135*pi && th_int(i) > -pi/2
bump(1)= 1; % Right sensor
elseif th_int(i) < 0.135*pi && th_int(i) > -0.135*pi
bump(2)= 1; % Front sensor (both left and right)
elseif th_int(i) < pi/2 && th_int(i) > 0.135*pi
bump(3)= 1; % Left sensor
end
end
% Ensure that front sensor doesn't read with other two
if bump(2)
bump(1)= 0;
bump(3)= 0;
elseif bump(1) && bump(3)
bump(1)= 0;
bump(2)= 1;
bump(3)= 0;
end
end
end
function cliff= genCliff(obj)
% cliff = genCliff(obj)
% Generates a reading for the cliff sensors
%
% Input:
% obj - Instance of class CreateRobot
%
% Output:
% cliff - Vector of doubles [right front-right front-left left],
% high values if there are no lines, low values for lines
% Create lines representing robot to look for intersections
% Get robot information
x_rob= obj.posAbs(1);
y_rob= obj.posAbs(2);
th_rob= obj.thAbs;
rad= obj.radius;
% Check against every obstacle (individually)
x_int= []; % Position of intersections
y_int= [];
for i= 1:size(obj.mapLines,1)% Count variable for obstacles
% Get line data
x1= obj.mapLines(i,1);
y1= obj.mapLines(i,2);
x2= obj.mapLines(i,3);
y2= obj.mapLines(i,4);
% Find intersection points on infinite lines
m= (y2-y1)/(x2-x1); % Slope of the line
if isinf(m) % Infinite slope
x_posInt= [x1 x2]; % Possible intersection points
y_posInt(1)= y_rob+sqrt(rad^2-(x1-x_rob)^2);
y_posInt(2)= y_rob-sqrt(rad^2-(x1-x_rob)^2);
else
b= y1-m*x1;
% Quadratic equation of form: Ax^2+Bx+C = 0
A= m^2+1;
B= 2*m*b-2*x_rob-2*m*y_rob;
C= x_rob^2+b^2-2*b*y_rob+y_rob^2-rad^2;
x_posInt(1)= (-B+sqrt(B^2-4*A*C))/(2*A);
x_posInt(2)= (-B-sqrt(B^2-4*A*C))/(2*A);
y_posInt(1)= m*x_posInt(1)+b;
y_posInt(2)= m*x_posInt(2)+b;
end
% Check that line intersects circle
if ~all(isreal(x_posInt)) || ~all(isreal(y_posInt))
x_posInt= []; % Eliminate all points
y_posInt= [];
else
elim= []; % Elements to eliminate
for j= 1:length(x_posInt) % Check both points
% Check that they are on the finite line
if(x_posInt(j) < min([x1 x2]) || ...
x_posInt(j) > max([x1 x2]) || ...
y_posInt(j) < min([y1 y2]) || ...
y_posInt(j) > max([y1 y2]))
elim= [elim j];
end
end
x_posInt(elim)= [];
y_posInt(elim)= [];
end
x_int= [x_int ; x_posInt];
y_int= [y_int ; y_posInt];
end
% Check that there are intersections
cliff= ones(1,4); % Initialize to no lines
if ~isempty(x_int)
% Change intersection points to local coordinates
x_int= x_int-x_rob;
y_int= y_int-y_rob;
% Get angle relative to robot front
th_int= obj.wrap2pi(atan2(y_int,x_int)-th_rob);
% Find sensor activated, if any
for i= 1:length(th_int)
if th_int(i) >= -0.37*pi && th_int(i) <= -0.295*pi
cliff(1)= 0; % Right sensor
elseif th_int(i) >= -0.105*pi && th_int(i) <= -0.04*pi
cliff(2)= 0; % Front right sensor
elseif th_int(i) >= 0.04*pi && th_int(i) <= 0.105*pi
cliff(3)= 0; % Front left sensor
elseif th_int(i) >= 0.295*pi && th_int(i) <= 0.37*pi
cliff(4)= 0; % Left sensor
end
end
end
% Get noise to change the reading of the sensors
if isfield(obj.noise,'cliff')
noiseAvg= obj.noise.cliff(1);
noiseStDev= obj.noise.cliff(2);
noiseVal= noiseAvg+noiseStDev*randn;
else
noiseVal= 0;
end
% Put in values for sensor reading
cliff= 20*cliff+1.5+noiseVal;
end
function wall= genIR(obj)
% wall = genIR(obj)
% Generates a reading for the infrared wall sensor
%
% Input:
% obj - Instance of class CreateRobot
%
% Output:
% wall - Boolean double, 1 if a wall is within range on the right
% of the robot
% Calculate position of sensor
sensAng= -0.225*pi; % Sensor placement relative to front of robot
x_sensor= obj.posAbs(1)+obj.radius*cos(obj.thAbs+sensAng);
y_sensor= obj.posAbs(2)+obj.radius*sin(obj.thAbs+sensAng);
% Solve for distance using general function
sensAngDir= -0.5*pi; % Sensor looks to the right
distIR= findDist(obj,x_sensor,y_sensor,...
obj.rangeIR,obj.thAbs+sensAngDir);
% Get noise to change the effective range of the infrared
if isfield(obj.noise,'wall')
noiseAvg= obj.noise.wall(1);
noiseStDev= obj.noise.wall(2);
noiseVal= noiseAvg+noiseStDev*randn;
else
noiseVal= 0;
end
% Generate reading
wall= distIR < obj.rangeIR+noiseVal;
end
function vwall= genVWall(obj)
% vwall = genVWall(obj)
% Generates a reading for the virtual wall sensor
%
% Input:
% obj - Instance of class CreateRobot
%
% Output:
% vwall - Boolean double, 1 if the robot is within the field of a
% virtual wall, or the "halo" surrounding the emitter
% Get sensor position
x_sensor= obj.posAbs(1)+obj.radius*cos(obj.thAbs);
y_sensor= obj.posAbs(2)+obj.radius*sin(obj.thAbs);
% Define virtual wall emitter constants
halo_rad= 0.45; % Radius of the halo around the emitter
range_short= 2.13; % Range of the wall on the 0-3' setting
ang_short= 0.33; % Angular range on the 0-3' setting
range_med= 5.56; % Range of the wall on the 4'-7' setting
ang_med= 0.49; % Angular range on the 4'-7' setting
range_long= 8.08; % Range of the wall on the 8'+ setting
ang_long= 0.61; % Angular range on the 8'+ setting
% Check against all virtual wall ranges
vwall= 0; % Initialize output to no-wall
for i= 1:size(obj.mapVWalls,1)
% Get emitter position
x_emit= obj.mapVWalls(i,1);
y_emit= obj.mapVWalls(i,2);
% Check if sensor is within halo
if sqrt((x_sensor-x_emit)^2+(y_sensor-y_emit)^2) < halo_rad
vwall_seen= 1;
else
% Get more emitter constants
th= obj.mapVWalls(i,3);
if obj.mapVWalls(i,4) == 1
range= range_short;
ang= ang_short;
elseif obj.mapVWalls(i,4) == 2
range= range_med;
ang= ang_med;
else
range= range_long;
ang= ang_long;
end
% Find points that define boundary of virtual wall
x_1= x_emit+range*cos(th+ang/2);
y_1= y_emit+range*sin(th+ang/2);
x_2= x_emit+range*cos(th-ang/2);
y_2= y_emit+range*sin(th-ang/2);
% Find if sensor is within virtual wall triangle
% Use determinant definition of the area of a triangle
area_vwall= 0.5*abs(det([x_1 y_1 1 ; x_2 y_2 1 ; ...
x_emit y_emit 1]));
area_1= 0.5*abs(det([x_1 y_1 1 ; x_2 y_2 1 ; ...
x_sensor y_sensor 1]));
area_2= 0.5*abs(det([x_emit y_emit 1 ; x_2 y_2 1 ; ...
x_sensor y_sensor 1]));
area_3= 0.5*abs(det([x_1 y_1 1 ; x_emit y_emit 1 ; ...
x_sensor y_sensor 1]));
area_tot= area_1+area_2+area_3;
vwall_seen= abs(area_tot-area_vwall) < 0.001;
end
% Check for walls between robot and emitter
dist_emit= sqrt((x_emit-x_sensor)^2+(y_emit-y_sensor)^2);
dist_vwall= findDist(obj,x_sensor,y_sensor,dist_emit,...
atan2(y_emit-y_sensor,x_emit-x_sensor));
if vwall_seen && dist_vwall == dist_emit
vwall= 1;
end
end
end
function distSonar= genSonar(obj)
% distSonar = genSonar(obj)
% Generates a reading for all sonar sensors
%
% Input:
% obj - Instance of class CreateRobot
%
% Output:
% distSonar - Vector of doubles [front left back right],
% distance along each line of sight to nearest obstacle
% Get noise parameters
if isfield(obj.noise,'sonar')
noiseAvg= obj.noise.sonar(1);
noiseStDev= obj.noise.sonar(2);
else
noiseAvg= 0;
noiseStDev= 0;
end
% Cycle through all sensors
distSonar= obj.rangeSonar*ones(1,4); % Preallocate for speed
for i= 1:4
% Calculate position and orientation of the sensor
% Assume sensors are at the edge of the robot
th_sensor= obj.thAbs+(i-1)*pi/2;
x_sensor= obj.posAbs(1)+obj.radius*cos(th_sensor);
y_sensor= obj.posAbs(2)+obj.radius*sin(th_sensor);
% Get noise value to change reading of sonar
noiseVal= noiseAvg+noiseStDev*randn;
% Solve for distance using general function
distSonar(i)= findDist(obj,x_sensor,y_sensor,...
obj.rangeSonar,th_sensor)+noiseVal;
end
end
function distLidar= genLidar(obj)
% distLidar = genLidar(obj)
% Generates a reading for the LIDAR sensor
%
% Input:
% obj - Instance of class CreateRobot
%
% Output:
% distLidar - Vector of doubles of length obj.numPtsLidar
% Distances correspond to angles [-angRangeLidar/2 angRangeLidar/2]
% Remember that angles are defined positive counter-clockwise
% Calculate position of sensor (same for all angles)
x_sensor= obj.posAbs(1)+obj.radius*cos(obj.thAbs);
y_sensor= obj.posAbs(2)+obj.radius*sin(obj.thAbs);
% Get noise parameters
if isfield(obj.noise,'lidar')
noiseAvg= obj.noise.lidar(1);
noiseStDev= obj.noise.lidar(2);
else
noiseAvg= 0;
noiseStDev= 0;
end
% Cycle through all points of sensing
distLidar= obj.rangeLidar*ones(1,obj.numPtsLidar);
for i= 1:obj.numPtsLidar
% Find orientation of line of sight
th_sensor= obj.thAbs+(i-1)*obj.angRangeLidar/...
(obj.numPtsLidar-1)-obj.angRangeLidar/2;
% Get noise value to change reading of LIDAR
noiseVal= noiseAvg+noiseStDev*randn;
% Solve for distance using general function
distLidar(i)= findDist(obj,x_sensor,y_sensor,...
obj.rangeLidar,th_sensor)+noiseVal;
% Set any readings below minimum to the minimum
distLidar(i)= max(distLidar(i),obj.rangeMinLidar);
end
end
function [ang dist color]= genCamera(obj)
% [ang dist color] = genCamera(obj)
% Generates the output from the blob detection on the camera,
% detects only beacons
%
% Input:
% obj - Instance of class CreateRobot
%
% Output:
% ang - Vector of doubles, each the angle relative to robot at
% which beacon is detected
% dist - Vector of doubles, each the distance of beacon from camera
% color - Matrix of doubles of width three (color vector),
% each row the color of beacon detected
% Get robot position and orientation
x_r= obj.posAbs(1);
y_r= obj.posAbs(2);
th_r= obj.thAbs;
% Check each beacon against camera ranges
ang= [];
dist= [];
color= [];
for i= 1:size(obj.mapBeacs,1)
% Get beacon position
x_b= obj.mapBeacs{i,1};
y_b= obj.mapBeacs{i,2};
% Find heading and distance from robot to beacon
ang_b= obj.wrap2pi(atan2(y_b-y_r,x_b-x_r)-th_r);
dist_b= sqrt((x_b-x_r)^2+(y_b-y_r)^2);
% See if there is a wall in the way
walldist= findDist(obj,x_r,y_r,dist_b,...
obj.wrap2pi(ang_b+th_r));
% If camera can see beacon, then save beacon information