function [sys,x0,str,ts] = gantry_msfun_3color(t,x,u,flag, handles)
%SFUNTMPL General M-file S-function template
% With M-file S-functions, you can define you own ordinary differential
% equations (ODEs), discrete system equations, and/or just about
% any type of algorithm to be used within a Simulink block diagram.
%
% The general form of an M-File S-function syntax is:
% [SYS,X0,STR,TS] = SFUNC(T,X,U,FLAG,P1,...,Pn)
%
% What is returned by SFUNC at a given point in time, T, depends on the
% value of the FLAG, the current state vector, X, and the current
% input vector, U.
%
% FLAG RESULT DESCRIPTION
% ----- ------ --------------------------------------------
% 0 [SIZES,X0,STR,TS] Initialization, return system sizes in SYS,
% initial state in X0, state ordering strings
% in STR, and sample times in TS.
% 1 DX Return continuous state derivatives in SYS.
% 2 DS Update discrete states SYS = X(n+1)
% 3 Y Return outputs in SYS.
% 4 TNEXT Return next time hit for variable step sample
% time in SYS.
% 5 Reserved for future (root finding).
% 9 [] Termination, perform any cleanup SYS=[].
%
%
% The state vectors, X and X0 consists of continuous states followed
% by discrete states.
%
% Optional parameters, P1,...,Pn can be provided to the S-function and
% used during any FLAG operation.
%
% When SFUNC is called with FLAG = 0, the following information
% should be returned:
%
% SYS(1) = Number of continuous states.
% SYS(2) = Number of discrete states.
% SYS(3) = Number of outputs.
% SYS(4) = Number of inputs.
% Any of the first four elements in SYS can be specified
% as -1 indicating that they are dynamically sized. The
% actual length for all other flags will be equal to the
% length of the input, U.
% SYS(5) = Reserved for root finding. Must be zero.
% SYS(6) = Direct feedthrough flag (1=yes, 0=no). The s-function
% has direct feedthrough if U is used during the FLAG=3
% call. Setting this to 0 is akin to making a promise that
% U will not be used during FLAG=3. If you break the promise
% then unpredictable results will occur.
% SYS(7) = Number of sample times. This is the number of rows in TS.
%
%
% X0 = Initial state conditions or [] if no states.
%
% STR = State ordering strings which is generally specified as [].
%
% TS = An m-by-2 matrix containing the sample time
% (period, offset) information. Where m = number of sample
% times. The ordering of the sample times must be:
%
% TS = [0 0, : Continuous sample time.
% 0 1, : Continuous, but fixed in minor step
% sample time.
% PERIOD OFFSET, : Discrete sample time where
% PERIOD > 0 & OFFSET < PERIOD.
% -2 0]; : Variable step discrete sample time
% where FLAG=4 is used to get time of
% next hit.
%
% There can be more than one sample time providing
% they are ordered such that they are monotonically
% increasing. Only the needed sample times should be
% specified in TS. When specifying than one
% sample time, you must check for sample hits explicitly by
% seeing if
% abs(round((T-OFFSET)/PERIOD) - (T-OFFSET)/PERIOD)
% is within a specified tolerance, generally 1e-8. This
% tolerance is dependent upon your model's sampling times
% and simulation time.
%
% You can also specify that the sample time of the S-function
% is inherited from the driving block. For functions which
% change during minor steps, this is done by
% specifying SYS(7) = 1 and TS = [-1 0]. For functions which
% are held during minor steps, this is done by specifying
% SYS(7) = 1 and TS = [-1 1].
% Copyright 1990-2002 The MathWorks, Inc.
% $Revision: 1.18 $
%
% The following outlines the general structure of an S-function.
%
persistent h
persistent tt
switch flag,
%%%%%%%%%%%%%%%%%%
% Initialization %
%%%%%%%%%%%%%%%%%%
case 0,
[sys,x0,str,ts,h]=mdlInitializeSizes(handles);
%%%%%%%%%%%%%%%
% Derivatives %
%%%%%%%%%%%%%%%
case 1,
sys=mdlDerivatives(t,x,u);
%%%%%%%%%%
% Update %
%%%%%%%%%%
case 2,
sys=mdlUpdate(t,x,u);
%%%%%%%%%%%
% Outputs %
%%%%%%%%%%%
case 3,
sys=mdlOutputs(t,x,u, handles, h);
%%%%%%%%%%%%%%%%%%%%%%%
% GetTimeOfNextVarHit %
%%%%%%%%%%%%%%%%%%%%%%%
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
%%%%%%%%%%%%%
% Terminate %
%%%%%%%%%%%%%
case 9,
sys=mdlTerminate(t,x,u);
%%%%%%%%%%%%%%%%%%%%
% Unexpected flags %
%%%%%%%%%%%%%%%%%%%%
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end
% end sfuntmpl
%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts,h]=mdlInitializeSizes(handles)
% video parameters
%exists separately in init and output in msfun, also in gantry init
videoRes=[240 320];
videoArea=[7.5 10]; %length of area video covers in m's
%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded. This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 0;
% *********************************** 3color
sizes.NumInputs = videoRes(1)*videoRes(2)*3+6; % times 3 for color : 6+image vector size
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
%
% initialize the initial conditions
%
x0 = [];
%
% str is always an empty matrix
%
str = [];
%
% initialize the array of sample times
%
ts = [.1 0];
%
% jk added 011505
% temp(:,:,1)=zeros(307);
% temp(:,:,2)=zeros(307);
% temp(:,:,3)=zeros(307);
%h.image=image(temp, 'Parent', handles.axes1);
graymap=[0:1/255:1 ; 0:1/255:1 ; 0:1/255:1]';
set(handles.figure1,'Colormap', graymap)
temp(:,:,1)=zeros(videoRes(1:2));
% *********************************************** 3color
temp(:,:,2)=zeros(videoRes(1:2)); % for color
temp(:,:,3)=zeros(videoRes(1:2));
hold(handles.axes1,'off');
% changed 062805
h.image=imagesc([0 videoArea(2)],[0 videoArea(1)], temp, 'Parent', handles.axes1); % use imagesc if color range never changes
%[0 videoArea(2)],[0 videoArea(1)],
%set(handles.axes1, 'Visible', 'off')
hold(handles.axes1,'on');
h.des1=plot(handles.axes1, 0, 0, 'o', 'MarkerSize',12, 'MarkerEdgeColor','r','MarkerFaceColor','w');
h.des2=plot(handles.axes1, 0, 0, 'o', 'MarkerSize',6, 'MarkerEdgeColor','r','MarkerFaceColor','r');
h.act=plot(handles.axes1, 0, 0, 'o', 'MarkerSize',10, 'MarkerEdgeColor','k','MarkerFaceColor','g');
% changed 062805
axis(handles.axes1,'manual');
axis(handles.axes1,[0 videoArea(2) 0 videoArea(1)]);%[6.7 18.7 6.7 18.7]);
%[0 320 0 240]
% for testing only 011905
%assignin('base','h',h);
% end mdlInitializeSizes
%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
function sys=mdlDerivatives(t,x,u)
sys = [];
% end mdlDerivatives
%
%=============================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
%
function sys=mdlUpdate(t,x,u)
sys = [];
% end mdlUpdate
%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u,handles,h)
%exists separately in init and output
videoRes=[240 320];
frameDataSize=videoRes(1)*videoRes(2);
% tic;
% ********************************* 3color
temp=zeros(videoRes(1),videoRes(2),3); % for color
temp(:,:,1)=reshape((u(7:6+frameDataSize)),videoRes(1),videoRes(2));
% ********************************** 3color
temp(:,:,2)=reshape((u(7+frameDataSize:6+frameDataSize*2)),videoRes(1),videoRes(2)); % for color
temp(:,:,3)=reshape((u(7+frameDataSize*2:6+frameDataSize*3)),videoRes(1),videoRes(2));
set(h.image, 'CData', temp);
% for testing only 062805
%assignin('base','temp',temp);
%tt=toc;
%mode_str=num2str(tt);
% temp=zeros(307,307,3);
% temp(:,:,1)=reshape(u(7:end),307,307);
% temp(:,:,2)=temp(:,:,1);
% temp(:,:,3)=temp(:,:,1);
% set(h.image, 'CData', temp);
u_new=u(1:4);
set(h.des1,'xdata',u_new(1));
set(h.des1,'ydata',u_new(2));
set(h.des2,'xdata',u_new(1));
set(h.des2,'ydata',u_new(2));
set(h.act,'xdata',u_new(3));
set(h.act,'ydata',u_new(4));
% Electromagnet on/off color
if u(5)>.5
set(h.act,'MarkerFaceColor', [1 0 0])
else
set(h.act,'MarkerFaceColor', [0 1 0])
end
% mode indicator
switch u(6)
case 0
mode_str='initializing';
case 1
mode_str='hold home position';
case 2
mode_str='seeking target';
case 3
mode_str='tracking target';
case 4
mode_str='picked up object';
case 5
mode_str='seeking home';
case 6
mode_str='waiting for drop cmd';
case 7
mode_str='hardware disabled';
case 8
mode_str='controller & hw disabled';
otherwise
mode_str='logic chart error';
end
set(handles.dynamic_plant_state,'String',mode_str);
sys = [];
% end mdlOutputs
%
%=============================================================================
% mdlGetTimeOfNextVarHit
% Return the time of the next hit for this block. Note that the result is
% absolute time. Note that this function is only used when you specify a
% variable discrete-time sample time [-2 0] in the sample time array in
% mdlInitializeSizes.
%=============================================================================
%
function sys=mdlGetTimeOfNextVarHit(t,x,u)
sampleTime = 1; % Example, set the next hit to be one second later.
sys = t + sampleTime;
% end mdlGetTimeOfNextVarHit
%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)
sys = [];
% end mdlTerminate
