sfdrawrobot.m

function sfdrawrobot(block)
%MSFUNTMPL A template for an M-file S-function
%   The M-file S-function is written as a MATLAB function with the
%   same name as the S-function. Replace 'msfuntmpl' with the name
%   of your S-function.  
%
%   It should be noted that the M-file S-function is very similar
%   to Level-2 C-Mex S-functions. You should be able to get more 
%   information for each of the block methods by referring to the
%   documentation for C-Mex S-functions.
%  
%   Copyright 2003-2006 The MathWorks, Inc.
%   $Revision: 1.1.6.10 $  
  
%%
%% The setup method is used to setup the basic attributes of the
%% S-function such as ports, parameters, etc. Do not add any other
%% calls to the main body of the function.  
%%   
setup(block);
  
%endfunction

%% Function: setup ===================================================
%% Abstract:
%%   Set up the S-function block's basic characteristics such as:
%%   - Input ports
%%   - Output ports
%%   - Dialog parameters
%%   - Options
%% 
%%   Required         : Yes
%%   C-Mex counterpart: mdlInitializeSizes
%%
function setup(block)

  % Register number of ports
  block.NumInputPorts  = 2;
  block.NumOutputPorts = 0;
  
  % Setup port properties to be inherited or dynamic
  block.SetPreCompInpPortInfoToDynamic;
  block.SetSimViewingDevice(1);

  % Override input port properties
  block.InputPort(1).DatatypeID  = 0;  % double
  block.InputPort(1).Complexity  = 'Real';
  
  block.InputPort(2).DatatypeID  = 0;  % double
  block.InputPort(2).Complexity  = 'Real';
  
  % Override output port properties
  %block.OutputPort(1).DatatypeID  = 0; % double
  %block.OutputPort(1).Complexity  = 'Real';

  % Register parameters
  block.NumDialogPrms     = 2;
  %block.DialogPrmsTunable = {'Tunable','Nontunable','SimOnlyTunable'};

  block.SetAccelRunOnTLC(false);
  
  block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup);
  block.RegBlockMethod('ProcessParameters', @ProcessPrms);
  block.RegBlockMethod('Start', @Start);
  block.RegBlockMethod('Update', @Update);
%endfunction

%% -------------------------------------------------------------------
%% The local functions below are provided for illustrative purposes
%% to show how you may implement the various block methods listed
%% above.
%% -------------------------------------------------------------------

function CheckPrms(block)
  
%   a = block.DialogPrm(1).Data;
%   if ~strcmp(class(a), 'double')
%     error('Invalid parameter');
%   end
  
%endfunction

function DoPostPropSetup(block)
  block.NumDworks             = 1;
  block.Dwork(1).Name         = 'plothandle';
  block.Dwork(1).Dimensions   = 5;
  block.Dwork(1).DatatypeID   = 0;
  block.Dwork(1).Complexity   = 'Real';
%endfunction

function Start(block)
    area = block.DialogPrm(1).Data.area;
    hFig = figure(1);
    set(hFig, 'Name', 'Robot');
    set(hFig, 'DoubleBuffer', 'on', 'Tag', 'robotScreen');
    %set(fig, 'Renderer', 'painters');
    %set(fig, 'Color', 'black');
    hold('on');
    fig_axes = get(hFig, 'CurrentAxes');
    block.Dwork(1).Data = zeros(size(block.Dwork(1).Data));
    drawStyle = block.DialogPrm(2).Data;
    if drawStyle==0,
        axis('equal');
        set(gca, 'XLim', [0,area.w]);
        set(gca, 'YLim', [0,area.h]);
        cla(fig_axes);
        hold on
    end
%endfunction

function hc = homog(coords)
    hc=[coords ones(size(coords, 1), 1)];
%endfunction

function Update(block)
    figure(1);
    desc = block.DialogPrm(1).Data;
    robot = desc.robot;
    x = block.InputPort(1).Data;
    y = block.InputPort(2).Data;
    
    outline=homog(desc.shape.outline);
    arrow=homog(desc.shape.arrow);
    
% Transformations
    mid = [
        1 0 -robot.cx
        0 1 -robot.cy
        0 0 1]';
    trans = [
        cos(x(3)) -sin(x(3)) x(1)
        sin(x(3))  cos(x(3)) x(2)
        0 0 1]';

    oldPlot = block.Dwork(1).Data;
    if any(oldPlot~=0), delete(oldPlot(oldPlot~=0)); end;
    
    outline=outline*mid*trans;
    arrow=arrow*mid*trans;
    drawStyle = block.DialogPrm(2).Data;
    if drawStyle==0,
        oldPlot(1) = fill(outline(:,1), outline(:,2), 'c');
        oldPlot(2) = plot(arrow(:,1), arrow(:,2), 'k');
    else
        oldPlot(1) = plot(outline(:,1), outline(:,2), drawStyle);
        oldPlot(2) = plot(arrow(:,1), arrow(:,2), drawStyle);
    end
    if any(y~=0),
        dir1 = homog(3*[robot.laserx robot.lasery; cos(y(1)) sin(y(1))]);
        dir2 = homog(3*[robot.laserx robot.lasery; cos(y(2)) sin(y(2))]);
        dir3 = homog(3*[robot.laserx robot.lasery; cos(y(3)) sin(y(3))]);
        d = [dir1*trans;
            dir2*trans;
            dir3*trans];
        oldPlot(3) = plot(d(:,1), d(:,2), 'r');
    end
    drawnow
    block.Dwork(1).Data = oldPlot;
    %endfunction

---