function [range_cm] = hokuyoRangeNavLab(s1, fireString, theta)
% s1 (serial port object) and fireString are set up by hokuyoRangeIntensity_Init
% return Range in CM, intensity in ???, and any error codes
% Theta has been established by hokuyoRangeIntensity_Init
maxedOut = 1000;
%Intensity = nan(N,M);
%AGC = nan(N,M);
% FIRE THE LASER!
fprintf(s1,fireString);
% laser is a string 'GXXXYYYZZ'
% G: Get range data
% XXX three digit # is starting angle in counts (min is 000)
% YYY three digit # is end angle (max is 768)
% ZZ two digits cluster size (returns min dist in cluster)
% count goes CCW, but dead until 44
% straight ahead is 384
% ends at 768 but dead after 725
% nominal angular range is 270 degrees
%
[N,M] = size(theta);
%err = nan(N,M);
range_cm = maxedOut*ones(N,M);
try % taking a scan
% Read Response
% need to make sure you read all data!
% ciel(769/inc) *2+Line feeds + status (1-2 numbers?) + command echo (9)
% expect a message 1576 characters long for full scan?
response = fread(s1,1);
while response~='G'
response = fread(s1,1)
end
response = fread(s1, 88);
%1st 8 are echo of original command, LF(8), error message(10?)?
% can error message be 2 digits?
if char(response(10))~='0'
disp('error taking scan?')
char(response(9))
char(response(10))
char(response(11))
%return % see manual, but their error messages don't make sense
end
% next line feed (code 10) begins raneg info
lineFeeds = find(10 == response);
nonLineFeeds = find(10 ~= response);
startIndexRange = lineFeeds(2)+1;
endIndexrange = length(response);
%removing all line feeds
RangeRaw = response(nonLineFeeds);
% starting after second line feed
% subtract offset of 30 base 16
RangeRaw = double(uint8(RangeRaw(startIndexRange-2:end)))-3*16;
% one range is two numbers representing second 6 bits, then 1st 6 bits
for i = 1:2:length(RangeRaw)-1
range_cm( (i+1)/2) = .1*(RangeRaw(i)*64 + RangeRaw(i+1));
% These are the cryptic error codes. 0-2
if range_cm( (i+1)/2) < 2
err((i+1)/2) = 10*range_cm( (i+1)/2);
if range_cm( (i+1)/2) == 0
% Says range might be less than min dist
% but I observe thsi is really a low intensity return
% code
range_cm( (i+1)/2) = maxedOut;
elseif (range_cm( (i+1)/2) == 1.6)||(range_cm( (i+1)/2) == 0.6)
% might be more than max dist
range_cm( (i+1)/2) = maxedOut;
elseif (range_cm( (i+1)/2) == .1)||(range_cm( (i+1)/2) == 0.2)||(range_cm( (i+1)/2) == .3)||(range_cm( (i+1)/2) == 0.4)||(range_cm( (i+1)/2) == 0.5)
% Hmmm this woudl include the "Low Light" Codes
range_cm( (i+1)/2) = maxedOut;
else
% may have error code in previous or last time just use
% value from prev step
if i~=1
range_cm( (i+1)/2) = range_cm( (i-1)/2);
else
range_cm( (i+1)/2) = maxedOut;
end
end
end
% Big digit is 1-5 small digit fluctuates up to 63
% Not sure these lines are correct
%Intensity( (i+5)/6) = (RangeRaw(i+2)*1024 + RangeRaw(i+3)*16 );
%AGC( (i+5)/6) = (RangeRaw(i+4)*64 + RangeRaw(i+5) );
end
catch
disp('Warning: Laser did not respond. Returning max values.');
end
disp('Flush buffer....')
N = s1.BytesAvailable();
while (N~=0)
fread(s1,N);
N = s1.BytesAvailable();
end