root/branches/workshop/Accelerator/GetAccelerator.m @ 100

function [acc_vector] = GetAccelerator(f_sensorport)

%% Build hex command and send it with NXT_LSWrite
    %-----------------------------LSWRITE----------------------------------

    % create this I2C command. basically all we want to do is request 6
    % bytes, the current sensor reading!
    
    RequestLen = 6;
    I2Cdata = hex2dec(['02'; '42']); 
    
        NXT_LSWrite(f_sensorport, RequestLen, I2Cdata, 'dontreply')
    
    %----------------------------------------------------------------------
    
    
%% Get current processing status of digital sensor    
    %---------------------------LSGETSTATUS--------------------------------;
    
    % we keep on asking the NXT when the ultrasonic sensor is ready...
    % note the dangerous potential of an infinite loop in case the sensor
    % should never get ready...
    
    %instead of "wasting time" in the idle loop below, waiting for the
    %sensor to get ready, we could also just wait some time here and
    %collect the sensor bytes then...
    %pause(0.015);
    
    
    BytesReady = 0;
    ticID = 34; %fixme randomly, dangerous
    timeout = 2; % in s
    tictic(ticID); 
    status = -1;
    %idlecount = 0;
    while ((BytesReady < 6) ||  (status ~= 0)) && (toctoc(ticID) < timeout)
        
        [BytesReady status] = NXT_LSGetStatus(f_sensorport);
        if status == 221 % communication bus error
            % recursive!
            acc_vector = GetAccelerator(f_sensorport);
            return
        end%if
        %idlecount = idlecount + 1;
    end%if
    
    %fprintf('idlecount = %d \n', idlecount)
%     if (status ~= 0) || (BytesReady < 6)
%         disp('whoa')
%     end%if
    
    acc_vector = [NaN NaN NaN];

%% If ready get object distance (ultrasonic sensor value)    
    if ( status == 0 ) && ( BytesReady > 0 ) 
        
        %-------------------------LSREAD-----------------------------------
        
        [data BytesRead] = NXT_LSRead(f_sensorport);
        
        % IMPORTANT, DON'T FORGET!!!
        % or change in NXT_LSRead
        data = double(data);
        
        if BytesRead > 5
            % as seen in the hitechnic example code...
            % this is just a byte shift and could be done better, but
            % should work
            if (data(1) > 127) 
                data(1) = data(1) - 256; 
            end
            if (data(2) > 127) data(2) = data(2) - 256; end
            if (data(3) > 127) data(3) = data(3) - 256; end
            acc_vector(1) = data(1) * 4 + data(4);
            acc_vector(2) = data(2) * 4 + data(5);
            acc_vector(3) = data(3) * 4 + data(6);
        end%if
        
        %------------------------------------------------------------------
    else
        [tmp msg] = checkStatusByte(status);
        fprintf('Invalid sensor reading, %d bytes received, status %d: %s\n', BytesReady, status, msg);
        
    end%if
    
end%function