root/branches/atorf/NewSensorDemos/AcceleratorPositionTracking2D.m @ 528

%% Set up Matlab
COM_CloseNXT all
close all
clear all
format compact


%% Set up ports

port  = SENSOR_1;



%% Get USB handle
myNXT = COM_OpenNXT;
COM_SetDefaultNXT(myNXT);


%% Open Sensor

OpenAccelerator(port);


%% Main loop

figure
hold on

% tic
% count = 0;
pos = [0; 0];
speed = [0; 0];

count = 100;
noise = zeros(3, 1);
for j = 1 : count
    tmp = GetAccelerator(port);
    noise = noise + tmp;
end%for

noise = noise ./ count;
%noise

j = 0;
tic
while (true)
    j = j + 1;
    
    % get accel
    acc = GetAccelerator(port);

    % get time period
    t = toc;
    tic % start again right away

    % substract noise
    acc = acc - noise;
    
    % throw away z component
    acc = acc(1:2);
    
    % invert x and y
    acc = flipud(acc);
    % invert sign of y
    acc(2) = -acc(2);
    
    % integrate for speed
    speed = speed + acc .* t; 

    % integrate for displacement
    % "classical"
    %pos = pos + speed .* t;
    % "physics-like"
    pos = pos + speed .* t + 0.5 .* acc .* t^2;

    if mod(j, 30) == 0
        % plot
        clf
        %plot(acc(1), acc(2), '.k')
        %plot(speed(1), speed(2), '.k')
        plot(pos(1), pos(2), '.k')
        limit = 100;
        axis([-limit limit -limit limit])
        drawnow
        pause(0.001)
    end%if
    
    % maybe show FPS
    if mod(j, 50) == 0
        disp(sprintf('Something like %.1f FPS', 1 / t))
    end%if

end%while


COM_CloseNXT(myNXT)