-
Notifications
You must be signed in to change notification settings - Fork 0
/
EXAMPLE1movement.m
106 lines (90 loc) · 3.31 KB
/
EXAMPLE1movement.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
clf; %clears figures
clc; %clears console
clear; %clears workspace
axis equal; %keeps the x and y scale the same
map=[0,0;60,0;60,45;45,45;45,59;106,59;106,105;0,105]; %default map
disp('You can set up a new BotSim object with the code:');
disp('botSim = BotSim(map)');
disp(' ');
botSim = BotSim(map); %sets up a botSim object a map, and debug mode on.
disp('As debug mode is on by default, you can set the angle and position of the robot');
disp('with setBotPos() and setBotAng(). During marking you will not be able');
input('to access these values. Press enter to initialise the robot:');
botSim.setBotPos([20 40])
botSim.setBotAng(pi)
disp('-------------------------------------------------------------------');
disp(' ');
disp('The current pose of the robot relative to the map is currently displayed');
disp('in figure 1.');
disp('In debug mode you can get the current bot position with getBotPos()');
botPosition = botSim.getBotPos()
disp('You can get the current bot angle in radians with getBotAng()');
botAngle = botSim.getBotAng()
botSim.drawMap();
botSim.drawBot(3);
disp('You can use the function turn() to turn the robot');
input('Press enter to make the robot turn pi/4 rad (45 deg)');
disp('-------------------------------------------------------------------');
disp(' ');
botSim.turn(pi/4);
hold off;
botSim.drawMap();
botSim.drawBot(3);
disp('you can use the function move(), to move the robot');
disp('forwards and backwards');
disp(' ');
input('Press enter to make the robot move forward 10cm');
disp('-------------------------------------------------------------------');
disp(' ');
botSim.move(10);
botSim.drawBot(3);
disp('You can combine turning and moving by calling both functions')
disp(' ');
input('Press enter to make the robot turn -pi/4 rad and move forward 30cm');
disp('-------------------------------------------------------------------');
disp(' ');
botSim.turn(-pi/4);
botSim.move(30);
botSim.drawBot(3);
disp('Note how the robot is able to leave the arena, no collision checking')
disp('is performed. You can call botSim.insideMap() to check if it is inside the map:')
insideMap = botSim.insideMap()
input('Press enter to reverse by 20cm')
disp('-------------------------------------------------------------------');
disp(' ');
botSim.move(-20);
botSim.drawBot(3);
disp('The insideMap function now returns 1')
insideMap = botSim.insideMap()
disp('You can set the motion noise level with the function setMotionNoise()');
input('Press enter to show the robot move from one location 50 times');
disp('-------------------------------------------------------------------');
disp(' ');
botSim.setBotPos([25 10]);
botSim.setBotAng(pi/2);
hold off
botSim.drawMap();
botSim.drawBot(3);
botSim.setMotionNoise(0.1);
for i = 1:50
botSim.setBotPos([25 10]);
botSim.move(60);
botSim.drawBot(3);
end
disp('You can also set the angular noise with the function setTurningNoise()');
input('Press enter to show the robot move from one location 50 times');
disp('-------------------------------------------------------------------');
disp(' ');
botSim.setBotPos([25 10]);
botSim.setBotAng(pi/2);
hold off
botSim.drawMap();
botSim.drawBot(3);
botSim.setTurningNoise(0.05);
for i = 1:50
botSim.setBotPos([25 10]);
botSim.setBotAng(pi/2);
botSim.move(60);
botSim.drawBot(3);
end
disp('Example1 finished');