function [sys,x0,str,ts] = eqm(t,x,u,flag)
% eqm
% 2 重倒立振子の運動方程式(非線形連立微分方程式)
% 質量等のパラメータは global 変数として宣言されているものを使用する。
%
% TIMESTWO S-function を編集して作成

%
% Dispatch the flag. The switch function controls the calls to 
% S-function routines at each simulation stage of the S-function.
%
switch flag,
  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  % Initialize the states, sample times, and state ordering strings.
  case 0
    [sys,x0,str,ts]=mdlInitializeSizes;

  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  % Return the outputs of the S-function block.
  case 3
    sys=mdlOutputs(t,x,u);

  %%%%%%%%%%%%%%%%%%%
  % Unhandled flags %
  %%%%%%%%%%%%%%%%%%%
  % There are no termination tasks (flag=9) to be handled.
  % Also, there are no continuous or discrete states,
  % so flags 1,2, and 4 are not used, so return an emptyu
  % matrix 
  case { 1, 2, 4, 9 }
    sys=[];

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Unexpected flags (error handling)%
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Return an error message for unhandled flag values.
  otherwise
    error(['Unhandled flag = ',num2str(flag)]);

end

% end eqm

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes()

sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 3;  % dynamically sized
sizes.NumInputs      = 5;  % dynamically sized
sizes.DirFeedthrough = 1;   % has direct feedthrough
sizes.NumSampleTimes = 1;

sys = simsizes(sizes);
str = [];
x0  = [];
ts  = [-1 0];   % inherited sample time

% end mdlInitializeSizes

%
%=============================================================================
% mdlOutputs
% Return the output vector for the S-function
%=============================================================================
%
function sys = mdlOutputs(t,x,u)

     global M m1 m2 l1 l2 g J1 J2

     A = [M+m1+m2, (m1+2*m2)*l1*cos(u(3)), m2*l2*cos(u(5));
	   (m1+2*m2)*l1*cos(u(3)), m1*l1^2+J1+4*m2*l1^2, 2*m2*l1*l2*cos(u(3)-u(5));
	   m2*l2*cos(u(5)), 2*m2*l1*l2*cos(u(3)-u(5)), m2*l2^2+J2];
     B = [u(1)+(m1+2*m2)*l1*u(2)^2*sin(u(3))+m2*l2*u(4)^2*sin(u(5));
	   (m1+2*m2)*g*l1*sin(u(3))-2*m2*l1*l2*u(4)^2*sin(u(3)-u(5));
	   m2*g*l2*sin(u(5))+2*m2*l1*l2*u(2)^2*sin(u(3)-u(5))];

sys = A\B;

% end mdlOutputs
