-
Notifications
You must be signed in to change notification settings - Fork 0
/
travelDist.m
executable file
·51 lines (43 loc) · 1.73 KB
/
travelDist.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
function travelDist(serPort, roombaSpeed, distance);
%travelDist(serPort, roombaSpeed, distance)
%Moves the Create the distance entered in meters. Positive distances move the
%Create foward, negative distances move the Create backwards.
%roombaSpeed should be between 0.025 and 0.5 m/s
% By; Joel Esposito, US Naval Academy, 2011
try
%Flush Buffer
N = serPort.BytesAvailable();
while(N~=0)
fread(serPort,N);
N = serPort.BytesAvailable();
end
warning off
global td
if (roombaSpeed < 0) %Speed given by user shouldn't be negative
disp('WARNING: Speed inputted is negative. Should be positive. Taking the absolute value');
roombaSpeed = abs(roombaSpeed);
end
if (abs(roombaSpeed) < .025) %Speed too low
disp('WARNING: Speed inputted is too low. Setting speed to minimum, .025 m/s');
roombaSpeed = .025;
end
if (distance < 0) %Definition of SetFwdVelRAdius Roomba, speed has to be negative to go backwards. Takes care of this case. User shouldn't worry about negative speeds
roombaSpeed = -1 * roombaSpeed;
end
if (distance ~=0)
SetFwdVelRadiusRoomba(serPort, roombaSpeed, inf);
distanceMM = distance * 1000;
fwrite(serPort, [156]); fwrite(serPort,distanceMM, 'int16');
pause(td)
SetFwdVelRadiusRoomba(serPort, 0, 0);
pause(td)
fwrite(serPort, [154]);
while( serPort.BytesAvailable() ==0)
disp('waiting to finish')
end
disp('Done travelDist.')
pause(td)
end
catch
disp('WARNING: function did not terminate correctly. Output may be unreliable.')
end