whereisit.m

此程序使用声音来定位物体的位置

function varargout=whereisit(varargin)
%WHEREISIT Determine location of speaker.
% WHEREISIT determines the location of a speaker which is placed between two
% microphones.  It is expected that the function CONFIGUREWHERISIT has been
% run prior to running this function.  WHEREISIT STOP will stop data from
% being collected and will also stop the sound from being played through the 
% speaker. WHEREISIT VR adds a nifty Virtual Reality (VRML) Visualization.
%
% See also CONFIGUREWHEREISIT, MAKEASOUND, TRACKER.

% This function is also used as a callback function for the analoginput
% object. When being called from the object, the calling syntax is 
% WHEREISIT(CallingObject,EventName,ConfigurationData,PlotHandles,BlockSize)

% Loren Dean (original version)
% Scott Hirsch (this version)

persistent vr 

switch nargin,
    % Initialization
case 0,
    vr =0;      %Don't use VR
    
    [fo,filename] = LInitialize(vr);

    if exist(filename,'file')
        makeasound(filename);
    else
        makeasound(fo);
    end;

    assignin('base','fo',fo);       %Push into base workspace for filter design
case 1,
    if strcmp(varargin{1},'vr')     %Initialize, with vr
        vr =1;  %Use VR
        [fo,filename] = LInitialize(vr);
        if exist(filename,'file')
            makeasound(filename);
        else
            makeasound(fo);
        end;
        assignin('base','fo',fo);       %Push into base workspace for filter design
    else            % Called as 'whereisit stop'
        makeasound stop
        
        Objects=daqfind('Name','Receive Sound');
        for lp=1:length(Objects),
            stop(Objects{lp});
        end
    end;
    % TimerAction Callback, Update the plot
otherwise,
    LPlot(varargin{:},vr)
    
end % switch
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%
%%%%% LInitialize %%%%%
%%%%%%%%%%%%%%%%%%%%%%%
function [fo,filename] = LInitialize(vr)
% Initialize the input object and start it.

% Load in the configuration data
if exist('whereisitconfiguration.mat','file'),
    load whereisitconfiguration
    %This is generated by configurewhereisit
    %The most important variables are LeftMic and RightMic, which are 
    % curve fits for sound level vs. distance from the two mics, and
    % cal, which is a calibration factor used to normalize the sensitivity
    % of the two mics
else,
    error('Please run the function CONFIGUREWHEREISIT before proceeding.')
end % if exist


% Set up the input object with two channels
% Use the sound card to acquire the data
AI=analoginput('winsound');
ch = addchannel(AI,1:2);


AI.SampleRate=44100; % Hz

%Apply calibration factor to channel 2.  Theoretically, both channels will
% now output the same amplitude when excited by the same source.
ch(2).SensorRange = ch(1).SensorRange *cal(2)/cal(1);

% Create the figure
samplesPerTrigger=1024*2;
handles=LCreateFigure(data,samplesPerTrigger,AI.SampleRate,LeftMic,RightMic,vr);

%Store information for band-pass filtering and smoothing
%We'll store this in AI's UserData
NAvg = 4;
FilterData.on = 0;              %Store current state of filter
FilterData.maxData = repmat(NaN,NAvg,2);     %Store last NAvgdata reading
FilterData.scale = [1 1];            %Scale factor - current_data / previous_data

% Configure the input object to callback every 0.1 seconds. The calling syntax
% will be: 
% WHEREISIT(CallingObject,EventName,ConfigurationData,PlotHandles,BlockSize,LeftMic,RightMic)
set(AI                  , ...
    'Name'               ,'Receive Sound', ...
    'SamplesPerTrigger'  ,samplesPerTrigger,  ...
    'TriggerRepeat'      ,inf  , ...
    'TimerFcn'           ,{'whereisit', data, handles, samplesPerTrigger,LeftMic,RightMic}, ...
    'TimerPeriod'        ,0.1,    ...
    'UserData'           ,FilterData ...
);   

% Start the object
start(AI)


%%%%%%%%%%%%%%%%%
%%%%% LPlot %%%%%
%%%%%%%%%%%%%%%%%
function LPlot(daqobj,event,configData,handles,blockSize,LeftMic,RightMic,vr)
% Callback function to plot the data for the input object.

% Check to see that enough data has been acquired.
NAvg=2;     %# Averages for location calculation. Time Series and fft will be on first only
if daqobj.SamplesAvailable>=NAvg*blockSize,
    data=peekdata(daqobj,NAvg*blockSize);
else,
    return
end % if samplesavailable

%I play with the shape of the data to make averaging easy

Data = reshape(data,blockSize,NAvg,2);     %Samples x Averages x Channel

%Get filter.  If user doesn't want to use a filter, we'll just use a unity gain.
try
    if get(handles.Filter,'Value')
        Num = get(handles.Figure,'UserData');
    else
        Num=1;
    end;
catch,
    whereisit stop
    return
end % try


DataF = filter(Num,1,Data,[],1);                %FIR Filter

data = squeeze(DataF(1:blockSize,1,:));         %Grab just the first block of data for time series, fft display

% Determine the magnitude of the acquired data
maxData = mean(squeeze(localRMS(DataF,1)));


%As soon as we turn the filter on, we will compare the magnitude
% to the stored value.  This will let us recalibrate our system
FilterData = get(daqobj,'UserData');
FilterWasOn = FilterData.on;                    %Previous state of filter
FilterIsOn = get(handles.Filter,'Value');    %Current state

prev_maxData = FilterData.maxData;          %Last NAvg values (most recent at bottom)
running_maxData = [prev_maxData(2:end,:);maxData];      %Add latest values to bottom of stack


if FilterIsOn & ~FilterWasOn        %Just turned on filter
    FilterData.scale = prev_maxData(end,:)./maxData;
elseif ~FilterIsOn & FilterWasOn    %Just turned off filter
    FilterData.scale = [1 1];
end;

%Update FilterData structure
FilterData.on = FilterIsOn; 
FilterData.maxData = running_maxData;   %Store for next time

%Apply scale factor to account for filter
maxData = nanmean(running_maxData) ./ FilterData.scale;
%nanmean is necessary, because we start with NaN's populating
% the maxData matrix


set(daqobj,'UserData',FilterData);


%Use fit to guess location
%Our fit:
%  y(x) = (p1) / (x + q1)
%  So, x(y) = p1/y - q1 
left = LeftMic.p1/maxData(1) - LeftMic.q1;      
right = 1 - (RightMic.p1/maxData(2) - RightMic.q1);       %1- so that starts at 1, goes to 0

barY(1,2) = left;
barY(1,1) = right;

% Make sure the bar data is clipped correctly between 0 and 1.
barY(barY<0)=0;
barY(barY>1)=1;

% Place the bar data into a cell array so that a vectorized set can be used
% below. 
barData={[0 barY(1)];[0 barY(2)]};


% Populate the lineData matrix and convert it to a cell array in order to be
% used in a vectorized set.
lineData=[barY(1) barY(2) 0]';
lineData(3)=mean(lineData(1:2));
position = mean(lineData(1:2));
lineData=num2cell(lineData);

% Calculate the FFT for both signals
fftData=abs(fft(data));
fftData(find(fftData==0))=1e-17;
fftData=20*log10(fftData);
fftData=fftData(1:blockSize/2,:);

% Attempt to put the data into the bar and line plots.  If this fails then
% the figure was closed and the command to stop the acquisition is given.
try,
    %    set(handles.loudspeaker_image,'XData',position + handles.loudspeaker_width_2*[-1 1]);
    %  set(handles.Bar,{'YData'},barData);
    set(handles.Line,{'Xdata'},lineData)
    set(handles.TimeSeries,{'YData'},num2cell(data(end/2+1:end,:),1)')
    set(handles.FFT,{'YData'},num2cell(fftData,1)')
catch,
    whereisit stop
    return
end % try

% Force the plots to update.
drawnow;



if vr
    %Update VR
    %Scale translation from x=0:1 to x=0:80 (VR scaling)
    Cobra_pos = handles.Cobra_pos;
    Cobra_pos(1) = lineData{3}*80;
    setfield(handles.Cobra,'translation',Cobra_pos); 
end;

% Flush the buffer if the acquisition gets ahead of the plotting.
if daqobj.SamplesAvailable>10000,
    flushdata(daqobj)
end % if

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% LCreateFigure %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
function h=LCreateFigure(data,blockSize,Fs,LeftMic,RightMic,vr)

% Determine if a "WHEREISIT" figure already exists.
nameStr='Acoustic Tracker';
hFigure=findobj('Name',nameStr);
% Either bring the existing one to the front
if ~isempty(hFigure),
    figure(hFigure)
    delete(get(hFigure,'Children'));
    
    % or create the figure
else,
    %   hFigure=figure('Units','normalized','Position',[.5 .5 .45 .45]);
%    hFigure=figure('Units','normalized','Position',[0.005    0.1    0.99    0.8]);
    hFigure=figure('Units','normalized','Position',[0.4902    0.0612    0.4854    0.4674]);  %Perfect for overlaying with PowerPoint title screen
    
    set(hFigure,'DefaultAxesFontSize',14,'DefaultAxesFontWeight','bold');
    
end % if

% Make sure the figure is configured correctly
set(hFigure      , ...
    'DoubleBuffer','on'   , ...
    'MenuBar'     ,'none' , ...
    'NumberTitle' ,'off'  , ...
    'Name'        ,nameStr  ...
);


%Grab a handle to the Tracker window to find out which products we have
set(0,'ShowHiddenHandles','on');
tracker_fig = findobj('Name','Tracker');
tracker_handles = guidata(tracker_fig);
has = tracker_handles.has;
set(0,'ShowHiddenHandles','off');



%Bottm of figure: Estimated source location.  Covers 5/6ths of width
hAxes = subplot(2,6,7:11);,

% Plot the first dot
h.Line=plot(0.5,0.25, ...
    'EraseMode'      ,'xor'  , ...
    'Marker'         ,'o'    , ...
    'MarkerSize'     ,10     , ...
    'MarkerFaceColor','blue' , ...
    'MarkerEdgeColor','black'  ...
    );
hAxes=get(h.Line,'Parent');

% Configure the axes
set(hAxes      , ...
    'DrawMode'  ,'fast'  , ...
    'XLim'      ,[0 1]   , ...
    'XTickLabel',{'Left','','','','','Center','','','','','Right'}, ...
    'XTick'     ,0:.1:1  , ...
    'XGrid'     ,'on'    , ...
    'YLim'      ,[0 1]   , ...
    'YGrid'     ,'on'    , ...
    'YTick'     ,0:.25:1, ...
    'YTickLabel',{'','Left','Average','Right',''} ...
    );

%Copy the first dot and make 2 new dots
h.Line(2:3)=copyobj(h.Line([1 1]),hAxes);
% Set the location and color of the 2 new dots.
set(h.Line(2),'MarkerFaceColor','red','YData',.75);
set(h.Line(3),'MarkerFaceColor','green','YData',.5, ...
    'MarkerSize',20);
   
% Place a title and xlabel on the axis
title(nameStr)

% Create the time series plots
YLim = max(max(data(1:2,:)));
subplot(5,2,1),
h.TimeSeries(1)=plot(ones(1,blockSize/2));
hAxes=get(h.TimeSeries(1),'Parent');
set(hAxes       , ...
    'YLim',[-YLim YLim], ...
    'XLim',[1 blockSize/2], ...
    'DrawMode'  ,'fast', ...
    'XTick',[0 precision(blockSize/2,-2)])
title('Time Series');

subplot(5,2,3),
h.TimeSeries(2)=plot(ones(1,blockSize/2));
hAxes=get(h.TimeSeries(2),'Parent');
set(hAxes       , ...
    'XLim',[1 blockSize/2], ...
    'YLim',[-YLim YLim], ...
    'DrawMode'  ,'fast', ...
    'XTick',[0 precision(blockSize/2,-2)])
xlabel('Time [Sec]')

% Create the FFT plots
df=Fs/blockSize;		%Frequency bin width
freq=(0:1:blockSize/2-1)*df;

subplot(5,2,2),
h.FFT(1)=plot(freq,ones(1,blockSize/2));
hAxes(1)=get(h.FFT(1),'Parent');
title('FFT')

subplot(5,2,4),
h.FFT(2)=plot(freq,ones(1,blockSize/2));
hAxes(2)=get(h.FFT(2),'Parent');
xlabel('Frequency [Hz]')  

set(hAxes      , ...
    'DrawMode'  ,'fast', ...
    'XLim'      ,[1 max(freq)], ...
    'YLim'     ,[-50 50] ...
    );
frwidth = .15;
frheight = .15;
frpos = [.8 .2 frwidth frheight];

Nui = 4;
uiwidth = .1;
uiheight = .03;
uipos =ones(Nui,1)*[frpos(1)+.5*(frwidth-uiwidth) frpos(2) uiwidth uiheight];
uivpos = frpos(2)+frheight - (1:Nui)'*uiheight-.02;
uipos(:,2) = uivpos;

frh = uicontrol('Style','Frame', ...
    'Units','norm', ...
    'Position',frpos);

FilterText = uicontrol('Style','text', ...
    'Units','norm', ...
    'String','Filter', ...
    'HorizontalAlignment','Center', ...
    'Position',uipos(1,:));

FilterDesign = uicontrol('Style','Pushbutton', ...
    'String','Design', ...
    'Units','norm', ...
    'Position',uipos(2,:), ...
    'Callback','fdatool');


%Update Filter pushbutton.  Pulls Num and Den from workspace into the figure's UserData
h.UpdateFilter = uicontrol('Style','Pushbutton', ...
    'String','Update', ...
    'Units','norm', ...
    'Position',uipos(3,:), ...
    'Callback','evalin(''base'',''set(gcf,''''UserData'''',Num)'',''Num=1;set(gcf,''''UserData'''',Num)'')');

%Use Filter checkbox.  Turns on the filter.
h.Filter = uicontrol('Style','checkbox', ...
    'String','Use', ...
    'Units','norm', ...
    'HorizontalAlignment','center', ...
    'Position',uipos(4,:) + [.025 0 -.05 0]);%, ...


if ~has.signal  %Disable filter stuff
    set([FilterText FilterDesign h.UpdateFilter h.Filter],'Enable','off', ...
        'TooltipString','Get the Signal Processing Toolbox to design and use a filter');
end;


Num=1;      %Default filter does nothing
set(hFigure,'UserData',Num);        %Initializing the filter
h.Figure=hFigure;


if vr
    %VR Toolbox Visualization
    if exist('tracker.wrl','file')
        myworld = vrworld('tracker.wrl');
        open(myworld)
%        setfield(myworld,'Description','Track the helicopter');
        view(myworld)
        mynodes = get(myworld, 'Nodes');
        Cobra = mynodes(1);
        Cobra_pos = getfield(Cobra,'translation');       %Need to keep track of original y and z coords
        
        h.Cobra = Cobra;
        h.Cobra_pos = Cobra_pos;
    end;
end;


function new=precision(old,prec)
%PRECISION        Change the numerical precision of a number
%
%  new=precision(old,prec);
%
% INPUTS:
%   old:		numbers you want to change. must be a matrix/scalar
%   prec:		number of digits to right of decimal
%
% OUTPUS:
%   new      old number, but with prec precision
%
% EX:  
%    old=12.3456;
%    prec=2;
%    new=precision(old,prec);
%     -->  new=12.35
% Scott Hirsch

new=round(old*10^(prec))/(10^(prec));

function out=localRMS(in,dim);
%RMS             Compute rms value of a steady signal

[nr,nc]=size(in);
if nargin<1
    help(mfilename);
elseif nargin<2
    [junk,dim]=max([nr nc]);
end;

out=sqrt(mean(in.^2,dim));