nxtscanner

function retData = nxtscanner
% main function <<nxtscanner program>>
retData = [];
data1 = [];
data2 = [];
holdFlag = 0;
breakFlag = 0;
nu = nxtusb;
if nu.CurrentState == 3
    open(nu);
    if nu.CurrentState == 5
        while breakFlag == 0
            buf = read(nu, 'uint16', 32);
            if nu.CurrentState == 10
                disp(['holdFlag: ', num2str(holdFlag)])
                % f[^
                [buf, holdFlag, directFlag, breakFlag] = extractData(buf, holdFlag);
                if ~isempty(buf)
                    if ~directFlag
                        % FROM
                        if ~isempty(data1)
                            retData = [retData; data1];
                            data1 = [];
                        end
                        data2= [buf, data2];
                        disp(['FROM data: ', num2str(buf)])
                    else
                        % TO
                        if ~isempty(data2)
                            retData = [retData; data2];
                            data2 = [];
                        end
                        data1 = [data1, buf];
                        disp(['TO data: ', num2str(buf)])
                    end
                else
                    disp('buf is empty!!!')
                end
            else
                close(nu);
                delete(nu);
                error('Data read error. Close NXT-USB interface.')
            end
        end
        % add last data
        if ~isempty(data1)
            disp(['Last data: ', num2str(data1)])
            retData = [retData; data1];
        end
        if ~isempty(data2)
            disp(['Last data: ', num2str(data2)])
            retData = [retData; data2];
        end
    else
        error('Cannot open NXT-USB interface')
    end
else
    error('Cannot find NXT. Check the NXT-USB connection.')
end
disp('Finish!!!!')
% -------------------------------------------------------------------------

% sub function for extracting data
% -------------------------------------------------------------------------
function [buf, holdFlag, directFlag, breakFlag] = extractData(buf, holdFlag)
disp('###### extractData START ######')
a = buf(1);
dataNum = buf(2);
validFlag = bitget(a, 1);
directFlag = bitget(a, 2);
endFlag = bitget(a, 3);
startFlag = bitget(a, 4);
breakFlag = 0;

tstr = ['dataNum: ' num2str(dataNum), ...
        ' validFlag: ' num2str(validFlag), ...
        ' directFlag: ' num2str(directFlag), ...
        ' startFlag: ' num2str(startFlag), ...
        'endFlag: ' num2str(endFlag)];
disp(tstr)

if validFlag
    if startFlag
        holdFlag = 1;
    end
    if endFlag
        breakFlag = 1;
    end
else
    buf = [];
    return;
end

if holdFlag
    if directFlag
        buf = buf(3:dataNum+2);
        disp('forward data!!')
    else
        % reverse the element order
        buf = buf(dataNum + 2:-1:3);
        disp('backward data!!')
    end
else
    buf = [];
end

disp('###### extractData END ######')
% -------------------------------------------------------------------------