function [sys,x0,str,ts] = csfuncTWOLINKMAN(t,x,u,flag,Xi)
% modelo no lineal
% x = Ax + Bu
% y = Cx
%TWO-LINK MANIPULATOR
%Autor:John Caipa
% Dispatch the flag.
%
switch flag,
case 0
[sys,x0,str,ts]=mdlInitializeSizes(Xi); % Initialization
case 1
sys = mdlDerivatives(t,x,u); % Calculate derivatives
case 3
sys = mdlOutputs(t,x); % Calculate outputs
case { 2, 4, 9 } % Unused flags
sys = [];
otherwise
error(['Unhandled flag = ',num2str(flag)]); % Error handling
end
% End of csfunc.
%==============================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the
% S-function.
%==============================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes(Xi)
%
% Call simsizes for a sizes structure, fill it in and convert it
% to a sizes array.
%
sizes = simsizes;
sizes.NumContStates = 4;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 4;
sizes.NumInputs = 2;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
%
% Initialize the initial conditions.
%
x0 = Xi;
%
% str is an empty matrix.
%
str = [];
%
% Initialize the array of sample times; in this example the sample
% time is continuous, so set ts to 0 and its offset to 0.
%
ts = [0 0];
% End of mdlInitializeSizes.
%
%==============================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%==============================================================
function sys = mdlDerivatives(t,x,u)
%valores parametros dados:
m1=0.5;
m2=0.5;
l1=0.5;
lc1=0.25;
lc2=0.25;
I1=0.083;
I2=0.033;
g=9.81;
m11=m1*lc1^2+m2*l1^2+I1;
m12=m2*l1*lc2;
m22=m2*lc2^2+I2;
k11=-(m1*lc1+m2*l1)*g;
k12=-m2*g*lc2;
sys(1) = x(3);
sys(2) = x(4);
sys(3) = (u(1)*m22+m12^2*cos(-x(1)+x(2))*sin(-x(1)+x(2))*x(3)^2+m12*cos(-x(1)+x(2))*k12*sin(x(2))-m12*cos(-x(1)+x(2))*u(2)+m12*sin(-x(1)+x(2))*x(4)^2*m22-k11*sin(x(1))*m22)/(m11*m22-m12^2*cos(-x(1)+x(2))^2);
sys(4) = -1/(m11*m22-m12^2*cos(-x(1)+x(2))^2)*(m12*cos(-x(1)+x(2))*u(1)+m11*m12*sin(-x(1)+x(2))*x(3)^2+m11*k12*sin(x(2))-m11*u(2)+m12^2*cos(-x(1)+x(2))*sin(-x(1)+x(2))*x(4)^2-m12*cos(-x(1)+x(2))*k11*sin(x(1)));
% End of mdlDerivatives.
%
%==============================================================
% mdlOutputs
% Return the block outputs.
%==============================================================
%
function sys = mdlOutputs(t,x)
sys = x;
% End of mdlOutputs.