www.gusucode.com > sl3ddemos工具箱matlab源码程序 > sl3ddemos/sl3d_sfuntraj.m

    function [sys, x0, str, ts, simStateCompliance] = sl3d_sfuntraj(t, x, u, flag, ax, Ts, mode)
%SL3D_SFUNTRAJ S-function for Trajectory scope with setpoint editor.
%   This MATLAB code is used as a Simulink S-function block. Block inputs
%   are X-Y coordinates which are used for plotting the trajectory.
%   By clicking the mouse inside the graph X-Y setpoint can be set 
%   which is the block output.

%   Copyright 1998-2013 HUMUSOFT s.r.o. and The MathWorks, Inc.


switch flag

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0
    [sys, x0, str, ts, simStateCompliance] = mdlInitializeSizes(ax, Ts);

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2
    sys = mdlUpdate(t, x, u, flag, ax, Ts, mode);

  %%%%%%%%%
  % Start %
  %%%%%%%%%
  case 'Start'
    LocalBlockStartFcn

  %%%%%%%%
  % Stop %
  %%%%%%%%
  case 'Stop'
    LocalBlockStopFcn

  %%%%%%%%%%%%%%
  % NameChange %
  %%%%%%%%%%%%%%
  case 'NameChange'
    LocalBlockNameChangeFcn

  %%%%%%%%%%%%%%%%%%%%%%%%
  % CopyBlock, LoadBlock %
  %%%%%%%%%%%%%%%%%%%%%%%%
  case { 'CopyBlock', 'LoadBlock' }
    LocalBlockLoadCopyFcn

  %%%%%%%%%%%%%%%
  % DeleteBlock %
  %%%%%%%%%%%%%%%
  case 'DeleteBlock'
    LocalBlockDeleteFcn

  % Unused flags %
  case { 1, 3, 9 }
    sys = [];

  % Other flags
  otherwise
    if ischar(flag),
      DAStudio.error('Simulink:blocks:unhandledFlag', flag);
    else
      DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
    end

end

% end sl3d_sfuntraj


%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys, x0, str, ts, simStateCompliance] = mdlInitializeSizes(ax, Ts)

if length(ax)~=4
  DAStudio.error('Simulink:blocks:axisLimitsMustBeDefined');
end

if ~strcmpi(vrgetpref('DataTypeBool'), 'logical')
  error(message('sl3d:demos:incompatibledatatypepref'));
end

sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 0;
sizes.NumInputs      = 4;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);

x0 = [];

str = [];

if ~isempty(Ts)
  ts = [Ts(1) 0];
else
  ts = [-1 0];
end

% specify that the simState for this s-function is same as the default
simStateCompliance = 'DefaultSimState';

model_name = bdroot(gcs);     % extract model name
wh = vrworld(get_param([model_name '/VR Sink'], 'WorldFileName'));
nh = vrnode(wh, 'MouseSensor');
sync(nh, 'hitPoint_changed', 'on');
sync(nh, 'isActive', 'on');

% end mdlInitializeSizes

%
%=============================================================================
% mdlUpdate
%=============================================================================
%
function sys = mdlUpdate(t, x, u, flag, ax, Ts, mode)   %#ok unused parameters

sys = [];

% Get UserData of the figure.
FigHandle = GetSfuntrajFigure(gcbh);
if ishandle(FigHandle)
  ud = get(FigHandle, 'UserData');
else
  ud = [];
end

% retrieve the setpoint
model_name = bdroot(gcs);
switch mode
case 1                   % mouse
  if ~isempty(ud)
    set(FigHandle, 'WindowButtonDownFcn', @LocalMouseButtonDownFcn);
    setpoint = [get(ud.Setpoint, 'XData') get(ud.Setpoint, 'YData')];
  else
    setpoint = [];
  end

case 2                    % signal
  if ~isempty(ud)
    set(FigHandle, 'WindowButtonDownFcn', '');
  end
  setpoint = u(3:4).';

case 3                    % VR sensor
  if ~isempty(ud)
    set(FigHandle, 'WindowButtonDownFcn', '');
  end
  wh = vrworld(get_param([model_name '/VR Sink'], 'WorldFileName'));
  nh = vrnode(wh, 'MouseSensor');
  if getfield(nh, 'isOver') && getfield(nh, 'isActive')   %#ok<GFLD> this is overloaded GETFIELD
    setpoint = getfield(nh, 'hitPoint_changed');          %#ok<GFLD> this is overloaded GETFIELD
    setpoint = setpoint([1 3]);
    % -z VRML axis is Matlab y axis
    setpoint(2) = -setpoint(2);
  else
    setpoint = [];
  end
end

% update the setpoint value, preserving model dirty flag
if ~isempty(setpoint)
  dirty = get_param(model_name, 'Dirty');
  set_param([gcs '/Setpoint'], 'Value', mat2str(setpoint));
  set_param(model_name, 'Dirty', dirty);
end

% return now if the figure does not exist
if isempty(ud)
  return;
end

% update the X/Y stored data points
ud.XYData(end+1, :) = u(1:2);
if size(ud.XYData, 1)==1
  plotdata = [u(1:2) u(1:2)];
else
  plotdata = ud.XYData(end-1:end, :);
end

% update the markers
set(ud.XYAxes, 'XLim', ax(1:2), 'YLim', ax(3:4));
set(ud.XYLineHead, 'XData', plotdata(end, 1), 'YData', plotdata(end, 2));
if ~isempty(setpoint) && mode~=1 % setpoint update not necessary in mouse mode
  set(ud.Setpoint, 'XData', setpoint(1), 'YData', setpoint(2));
end

% plot animated line - HG2 way
if ~isnumeric(ud.XYLine)
  if size(ud.XYData, 1)==1
    clearpoints(ud.XYLine);
  end
  if size(plotdata, 1)<2 || any(abs(plotdata(end, :)-plotdata(end-1, :)) > 0.01)
    addpoints(ud.XYLine, plotdata(end, 1), plotdata(end, 2));
  end

% plot animated line - HG1 way
else
  set(ud.XYLine, 'XData', plotdata(:, 1), 'YData', plotdata(:, 2));
end

set(FigHandle, 'UserData', ud);
drawnow;

% end mdlUpdate

%
%=============================================================================
% LocalBlockStartFcn
% Function that is called when the simulation starts.  Initialize the
% Trajectory Graph scope figure.
%=============================================================================
%
function LocalBlockStartFcn

% get the figure associated with this block, create a figure if it doesn't
% exist
FigHandle = GetSfuntrajFigure(gcbh);
if ~ishandle(FigHandle)
  FigHandle = CreateSfuntrajFigure;
end
ud = get(FigHandle, 'UserData');

% clear the previous line
ud.XYData = zeros(0, 2);
if ~isnumeric(ud.XYLine) 
  clearpoints(ud.XYLine);
else
  set(ud.XYLine, 'EraseMode', 'normal', 'XData', [], 'YData', []);
  set(ud.XYLine, 'EraseMode', 'none');
end

% initialize setpoint
xl = get(ud.XYAxes, 'XLim');
yl = get(ud.XYAxes, 'YLim');
setpoint = [(xl(1)+xl(2)) (yl(1)+yl(2))]./2;
set(ud.Setpoint, 'XData', setpoint(1));
set(ud.Setpoint, 'YData', setpoint(2));
dirty = get_param(bdroot(gcbh), 'Dirty');
set_param([gcb '/Setpoint'], 'Value', mat2str(setpoint));
set_param(bdroot(gcbh), 'Dirty', dirty);

set(FigHandle, 'UserData', ud);

% end LocalBlockStartFcn

%
%=============================================================================
% LocalBlockStopFcn
% At the end of the simulation, set the line's X and Y data to contain
% the complete set of points that were acquire during the simulation.
% Recall that during the simulation, the lines are only small segments from
% the last time step to the current one.
%=============================================================================
%
function LocalBlockStopFcn

FigHandle = GetSfuntrajFigure(gcbh);
if ishandle(FigHandle)
  ud = get(FigHandle, 'UserData');
  if isnumeric(ud.XYLine)
    set(ud.XYLine, 'XData', ud.XYData(:, 1), 'YData', ud.XYData(:, 2));
  end
end

% end LocalBlockStopFcn

%
%=============================================================================
% LocalBlockNameChangeFcn
% Function that handles name changes on the Graph scope block.
%=============================================================================
%
function LocalBlockNameChangeFcn

% get the figure associated with this block, if it's valid, change
% the name of the figure
FigHandle = GetSfuntrajFigure(gcbh);
if ishandle(FigHandle)
  set(FigHandle, 'Name', get_param(gcbh, 'Name'));
end

% end LocalBlockNameChangeFcn

%
%=============================================================================
% LocalBlockLoadCopyFcn
% This is the XYGraph block's LoadFcn and CopyFcn.  Initialize the block's
% UserData such that a figure is not associated with the block.
%=============================================================================
%
function LocalBlockLoadCopyFcn

SetSfuntrajFigure(gcbh, -1);

% end LocalBlockLoadCopyFcn

%
%=============================================================================
% LocalBlockDeleteFcn
% This is the XY Graph block'DeleteFcn.  Delete the block's figure window,
% if present, upon deletion of the block.
%=============================================================================
%
function LocalBlockDeleteFcn

% Get the figure handle associated with the block, if it exists, delete
% the figure.
FigHandle = GetSfuntrajFigure(gcbh);
if ishandle(FigHandle)
  delete(FigHandle);
  SetSfuntrajFigure(gcbh, -1);
end

% end LocalBlockDeleteFcn

%
%=============================================================================
% LocalFigureDeleteFcn
% This is the XY Graph figure window's DeleteFcn.  The figure window is
% being deleted, update the XY Graph block's UserData to reflect the change.
%=============================================================================
%
function LocalFigureDeleteFcn(this, eventdata) %#ok<INUSD> eventdata unused

% Get the block associated with this figure and set its figure to -1
ud = get(this, 'UserData');
SetSfuntrajFigure(ud.Block, -1);

% end LocalFigureDeleteFcn

%
%=============================================================================
% LocalMouseButtonDownFcn
% Sends mouse coordinates to block output.
%=============================================================================
%
function LocalMouseButtonDownFcn(this, eventdata)

set(gcbf, 'WindowButtonMotionFcn', @LocalMouseFcn, ...
          'WindowButtonUpFcn',     @LocalMouseButtonUpFcn);
LocalMouseFcn(this, eventdata)

%
%=============================================================================
% LocalMouseButtonUpFcn
% Sends mouse coordinates to block output.
%=============================================================================
%
function LocalMouseButtonUpFcn(this, eventdata)

set(gcbf, 'WindowButtonMotionFcn', '', ...
          'WindowButtonUpFcn',     '');
LocalMouseFcn(this, eventdata)

%
%=============================================================================
% LocalMouseFcn
% Sends mouse coordinates to block output.
%=============================================================================
%
function LocalMouseFcn(this, eventdata) %#ok<INUSD> eventdata unused

ud = get(this, 'UserData');
mouse = get(get(ud.Setpoint, 'Parent'), 'CurrentPoint');
mouse = mouse(1, 1:2);
Xlim = get(ud.XYAxes, 'XLim');
Ylim = get(ud.XYAxes, 'YLim');
if (mouse(1)>=Xlim(1) && mouse(1)<=Xlim(2) && mouse(2)>=Ylim(1) && mouse(2)<=Ylim(2))
  set(ud.Setpoint, 'XData', mouse(1), 'YData', mouse(2));
end
drawnow;

% end LocalMouseFcn

%
%=============================================================================
% GetSfuntrajFigure
% Retrieves the figure window associated with this S-function XY Graph block
% from the block's parent subsystem's UserData.
%=============================================================================
%
function FigHandle = GetSfuntrajFigure(block)

if strcmp(get_param(block, 'BlockType'), 'S-Function'),
  block = get_param(block, 'Parent');
end

FigHandle = get_param(block, 'UserData');
if isempty(FigHandle),
  FigHandle=-1;
end

% end GetSfuntrajFigure

%
%=============================================================================
% SetSfuntrajFigure
% Stores the figure window associated with this S-function XY Graph block
% in the block's parent subsystem's UserData.
%=============================================================================
%
function SetSfuntrajFigure(block, FigHandle)

if strcmp(get_param(bdroot, 'BlockDiagramType'), 'model'),
  if strcmp(get_param(block, 'BlockType'), 'S-Function'),
    block = get_param(block, 'Parent');
  end

  set_param(block, 'UserData', FigHandle);
end

% end SetSfuntrajFigure

%
%=============================================================================
% CreateSfuntrajFigure
% Creates the figure window associated with this S-function XY Graph block.
%=============================================================================
%
function FigHandle = CreateSfuntrajFigure

% the figure doesn't exist, create one
screenLoc = get(0, 'ScreenSize');
origin = max(-screenLoc(1:2), 0) + 100;
FigHandle = figure('Units',          'pixels', ...
                   'Position',       [origin 400 300], ...
                   'Name',           get_param(gcbh, 'Name'), ...
                   'Tag',            'TRAJSCOPE', ...
                   'NumberTitle',    'off', ...
                   'IntegerHandle',  'off', ...
                   'ToolBar',        'none', ...
                   'MenuBar',        'none', ...
                   'DeleteFcn',      @LocalFigureDeleteFcn);

% store the block's handle in the figure's UserData
ud.Block = gcbh;

% create various objects in the figure
ud.XYAxes = axes;

% create the markers
ud.XYLineHead = line(0, 0, 'Marker', 'o', 'MarkerEdgeColor', 'r');
ud.Setpoint = line(0, 0, 'Marker', 'o', 'MarkerEdgeColor', 'g');

% create the animated line
try
  % try the HG2 line animator first
  ud.XYLine = animatedline('Parent', ud.XYAxes, ...
                           'Color', 'b', ...
                           'MaximumNumPoints', 1000);

catch ME %#ok<NASGU>
  % use plain line if the above failed
  ud.XYLine = line(0, 0, 'LineStyle', '-', 'Color', 'b');
  
  % we can use EraseMode on HG1
  if isnumeric(ud.XYAxes)
    set(ud.XYLine, 'EraseMode', 'none');
    set(ud.XYLineHead, 'EraseMode', 'xor');
    set(ud.Setpoint, 'EraseMode', 'xor');
  end
end

title('Trajectory Graph');
xlabel('X Axis');
ylabel('Y Axis');
ud.XYData   = zeros(0,2);

% Associate the figure with the block, and set the figure's UserData.
SetSfuntrajFigure(gcbh, FigHandle);
set(FigHandle, 'HandleVisibility', 'callback', 'UserData', ud);

% end CreateSfuntrajFigure