Introduction
Aim of this toolbox
Aim of this toolbox is to provide a welldocumented, easymodifiable tools for controlling mobile robots with Simulink, that will provide a base for developing user's own control algorithms. Documentation also includes introduction to writing Level 1 Mfile Sfunctions.
Intended audience
User of this toolbox should be familiar with basics of Matlab and Simulink.
Introduction into Sfunctions programming
Note: this short introduction will cover only essential topics on sfunctions, particularly about discrete state, level1, mfile sfunctions. You should refer to simulink's online documentation for more information.
General sfunction structure
Mfile sfunctions are not very different from ordinary mfile script as used on matlab's command line, but it has to have specific construction: (see "toolbox\simulink\blocks\dsfunc.m" for full version of the file below). It is recommended that you use that file as a template for your functions, and it is also recommended, that you do not change keywords used here (such as sys, mdlUpdate, mdlOutputs) as this will make your sfunction easier to comprehend by others.
function [sys,x0,str,ts]=s_function_template(t,x,u,flag,param)
switch flag,
case 0, % simulink calls for initialization
[sys,x0,str,ts]=sf_Init(param);
case 2, % simulink calls for update internal states
sys=mdlUpdate (t,x,u,param);
case 3, % simulink calls for update outputs
sys=mdlOutputs(t,x,u,param);
case 'CustomFlag',
processCustomFlag(param);
case { 1,2, 4, 9 }, % unused flags
sys = [];
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end % switch
end % function
function [sys,x0,str,ts]=sf_Init(param);
% standard Sfunction initialization
% instruct simulink about basic properties of this block
% and return them in "sys"
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 1;
sizes.NumInputs = 0;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 = [];% initialize the initial conditions
str = []; % str is always an empty matrix  required;
ts = [0.1 0]; % specify sampletime and offset
register_callbacks('s_function_template(0,0,0,''CustomFlag'',param)');
% attention! notice double quotation marks '' on the example above
rest_of_custom_init_code;
end %% initialization
function sys=mdlOutputs(t,x,u,param)
% t is time, x is internal state vector, u is input vector,
% sys is output vector, param is parameter vector
sys=t+x*param+u; % note that empty matrix [] would mean "output not changed"
% sys becomes output after this;
% note that sizes.DirFeedthrough = 1; in initialization because sys calculation uses u here
end
function sys= mdlUpdate(t,x,u,param);
% this routine is only called once per samplehit
sys=u; % note that empty matrix [] would mean "internal state not changed"
% note that sys becomes x after this,
end
function processCustomFlag(param);
% this kind of procedure is usually used for processing callbacks
end 
First of all, your sfunction must be able to handle at least two distinct events originating from simulink simulation phases: Initialization and simulation loop (there are also other phases, but they are out of scope of this introduction. For detailed information type "How SFunctions Work" in simulink help). Therefore, sfunction must provide simulink with at least two subfunctions: initialization and output update. The differentiation between these is achieved by using "flag" flag in
function [sys,x0,str,ts]=s_function_template(t,x,u,flag,param) 
and then by selecting appropriate sub function with
switch flag,
case . . . otherwise . . . end 
Flag "0" means initialization, flag "3" is update outputs, and so on. "Type Implementing SFunctions" in simulink help for full list.
Initialization subfunction
During the initialization phase, sfunction must provide simulink with basic information about itself. Initialization subfunction is also called when dropping a new sfunction into a model. See example above.
Most of that code is selfexplanatory. NumContStates, NumDiscStates informs simulink about length the of a internal state vector of continious or discrete states of your sfunction, respectively. For this toolbox, we use only discrete states, if any. If you specify any of these for more than 0, you will need to provide subfunction for flag "2" (update internal states) returning a vector of exactly that length.
It is important to specify if the function is an directfeedtrough type or not. It is directfeedtrough if you use "u" directly to calculate the outputs. Example of directfeedtrough function is presented above. A nondirectfeedtrough function would be delay and bias (where x=u; y=x+param; ) or integrator squareroot (where x=x+u; y=x.^0.5; ) (x is internal state vector) . It is important because simulink may call directfeedtrough functions several times per sample hit looking for zero crossing, linearizing, or estimating output change rate when the variablestep solver is used. That's why it is also important that if your sfunction has internal states, you are only allowed (and given chance) to update them when called by flag "2" (update internal states).
The next thing to pay attention for, is sample rate specification:
ts = [0.1 0]; % specify sampletime and offset 
First value (in seconds) defines sample rate. 1 means inherited (driven by block attached to input port). Second value means offset from each sample hit. It can be used to specify the block execution order, and it have important use in multirate systems, but usually it is enough to leave it at zero (all blocks will execute as connected, so their input data will be always uptodate).
If you use an sfile to communicate with a user interface, you will probably need to register some callbacks. Do this in this initialization subroutine. Use 'CustomFlag' to direct code execution to your callback processing subroutine. Pass any parameters using 'param' field.
register_callbacks('s_function_template(0,0,0,''CustomFlag'',param)'); 
After going trough initialization, you can start to write your custom code. Use mdlUpdate to update internal states and mdlOutputs to calculate output. Unfortunatelly, level1 Mfiles do not support variable length input or output, so your subfunction must always return a vector of length specified by sizes.NumOutputs. Output can be also set to empty (sys=[]), which means "do not change output, hold it to previous state".
Simulation loop subfunciton
These subfunctions are invoked every time step during simulation:
function sys= mdlUpdate(t,x,u,param);
% this routine is only called once per samplehit
sys=u; % note that empty matrix [] would mean "internal state not changed"
% note that sys becomes x after this,
end
function sys=mdlOutputs(t,x,u,param)
% t is time, x is internal state vector, u is input vector,
% sys is output vector, param is parameter vector
sys=t+x*param+u; % note that empty matrix [] would mean "output not changed"
% sys becomes output after this;
% note that sizes.DirFeedthrough = 1; in initialization because sys calculation uses u here
end

You define what should they do. If you not use them, you can specify their flags as unused in the main function:
case { 1,2, 4, 9 }, % unused flags
sys = []; 
Note that these subfunctions can return either a valid length vector ( specified by sizes.NumDiscStates=0; and sizes.NumOutputs=1;) or an empty matrix ( sys=[]; ) which would mean "preserve last output".
Working environment and general assumptions
Coordinate system used and angle calculation
We use image coordinate system in Visualisation window:
Note that angle is represented in radians, and it should be in range (pi; +pi); You should wrap angles extending beyond this range  use "wrap angle" block. Also note that straight upwards direction has double representation +pi and pi; this is only a problem when calculating derivatives (angular velocity)  you should use "unwrap angle" block before calculating angle derivative. This should not be a problem when integrating an angle because you usually would integrate only angle difference (for PI contoller).
You can use the "calculate distance and angle" block to calculate distance and direction between two points.
Simulation enviroment
Most functions are built around the main visualization window.
Every single functions calls robo_vis_init during the initialization phase. This function does a check to ensure the visualization window exists. If it exists, it does nothing. If visualization window doesn't exist, it initializes it. After that, calling function creates appropriate objects, and finishes it's initialization phase. During simulation loop, in every call, the visualization function looks for the visualization window, gets its handle, retrieves stored objects, modifies them, and stores them again. See point 4.3. for details.
Global variables storage
Level 1 M Sfunctions are not allowed to use global or persistent variables. Instead, they can use work vector of fixed size (which is private to particular function block) or use external objects to store and access global data. In MoRoTeCo toolbox we use the visualization's window "UserData" variable to store "allobj" object, which will contain all simulationwide data.
Look at the example of the code below:
fig = findobj('Tag','Robo_Vis'); %find handle to vis window
allobj=get(fig,'UserData'); %standard start: get stored data
allobj.command_point_current_button=2; % remember some data set(fig,'UserData',allobj); % standard end: store our data to vis window. 
This way data is associated with visualization window, and can be recalled by any block in the model.
