You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
44 lines
1.2 KiB
44 lines
1.2 KiB
function [] = SetFwdVelRadiusRoomba(serPort, FwdVel, Radius);
|
|
%[] = SetFwdVelRadiusRoomba(serPort, FwdVel, Radius)
|
|
|
|
% Moves Roomba by setting forward vel and turn radius
|
|
% serPort is a serial port object created by Roombainit
|
|
% FwdVel is forward vel in m/sec [-0.5, 0.5],
|
|
% Radius in meters, postive turns left, negative turns right [-2,2].
|
|
% Special cases: Straight = inf
|
|
% Turn in place clockwise = -eps
|
|
% Turn in place counter-clockwise = eps
|
|
|
|
% 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
|
|
%% Convert to millimeters
|
|
FwdVelMM = min( max(FwdVel,-.5), .5)*1000;
|
|
if isinf(Radius)
|
|
RadiusMM = 32768;
|
|
elseif Radius == eps
|
|
RadiusMM = 1;
|
|
elseif Radius == -eps
|
|
RadiusMM = -1;
|
|
else
|
|
RadiusMM = min( max(Radius*1000,-2000), 2000);
|
|
end
|
|
|
|
%fwrite(serPort, [137, 255, 56, 1, 244])
|
|
|
|
fwrite(serPort, [137]); fwrite(serPort,FwdVelMM, 'int16'); fwrite(serPort,RadiusMM, 'int16');
|
|
disp('moving!')
|
|
pause(td)
|
|
catch
|
|
disp('WARNING: function did not terminate correctly. Output may be unreliable.')
|
|
end
|
|
|