% Find distance
% MapBot Project    Stephanie Luft
% This function finds the distance to an obstacle from the picture taken by
% the webcam.  If the pixels from the center (pfc) is outside of the range 
% permitted by the equation, the distance is set to -1. Later routines flag
% this as an anomoly before plotting.

function [distance]=finddistance(pic)

pfcpic=pic(250:258,1:320);  % Look at the window in the picture where the laser dot is expected to be
maxval=0;
pfc=0;
for i=1:6
    for j=1:320
        if pfcpic(i,j)>maxval   % if the new pixel is the brightest so far
            pfc=320-j;          % then calculate pfc
            maxval=pfcpic(i,j);
        end
    end
end
distance=.189/tan(.0013923*pfc-.0324705);   
    % this equation was found as the result of testing with the laser distance 
    % meter.  If a different setup were used, or for higher accuracy in different 
    % ranges, a different equation or different coefficients would be used.
if pfc<31 | pfc>282     % if pfc is outside of plausible range
    distance=-1;
end