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