User Guide

Introduction

Aim of this toolbox

Aim of this toolbox is to provide a well-documented, easy-modifiable 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 M-file S-functions.

Intended audience

User of this toolbox should be familiar with basics of Matlab and Simulink.

Introduction into S-functions programming

Note: this short introduction will cover only essential topics on s-functions, particularly about discrete state, level-1, mfile s-functions. You should refer to simulink's online documentation for more information.

General s-function structure

 

Mfile s-functions are not very different from ordinary m-file 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 s-function 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 S-function 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 s-function 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 S-Functions Work" in simulink help). Therefore, s-function 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 S-Functions" in simulink help for full list.

Initialization subfunction

During the initialization phase, s-function must provide simulink with basic information about itself. Initialization subfunction is also called when dropping a new s-function into a model. See example above.

Most of that code is self-explanatory. NumContStates, NumDiscStates informs simulink about length the of a internal state vector of continious or discrete states of your s-function, 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 direct-feedtrough type or not. It is direct-feedtrough if you use "u" directly to calculate the outputs. Example of direct-feedtrough function is presented above. A non-direct-feedtrough function would be delay and bias (where x=u; y=x+param; ) or integrator square-root (where x=x+u; y=x.^0.5; ) (x is internal state vector) . It is important because simulink may call direct-feedtrough functions several times per sample hit looking for zero crossing, linearizing, or estimating output change rate when the variable-step solver is used. That's why it is also important that if your s-function 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 up-to-date).

If you use an s-file 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, level-1 M-files 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 S-functions 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 simulation-wide 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.