% THE IRB 140 ROBOT MANIPULATOR BLOCK (SYSTEM CONFIGURATION + DYNAMIC MODELING)
function irb140(block)
%% The setup method is used to setup the basic attributes of the
%% S-function such as ports, parameters, etc.
setup(block);
%endfunction
function setup(block)
% Register number of ports
block.NumInputPorts = 2;
block.NumOutputPorts = 1;
% Setup port properties to be inherited or dynamic
block.SetPreCompInpPortInfoToDynamic;
block.SetPreCompOutPortInfoToDynamic;
% Override input port properties
block.InputPort(1).Dimensions = [12 1];
block.InputPort(2).Dimensions = [6 1];
block.InputPort(1).DirectFeedthrough = false;
block.InputPort(2).DirectFeedthrough = false;
% Override output port properties
block.OutputPort(1).Dimensions = [12 1];
% Register sample times
block.SampleTimes = [-1 0];
%% -----------------------------------------------------------------
%% Options
%% -----------------------------------------------------------------
% Specify if Accelerator should use TLC or call back into
% M-file
block.SetAccelRunOnTLC(false);
%% -----------------------------------------------------------------
%% Register methods
%% -----------------------------------------------------------------
block.RegBlockMethod('InitializeConditions', @InitializeConditions);
block.RegBlockMethod('Outputs', @Outputs);
%endfunction
%% -------------------------------------------------------------------
%% The local functions
%% -------------------------------------------------------------------
function InitializeConditions(block)
block.InputPort(1).Data(1) = 0;
block.InputPort(1).Data(2) = 0;
block.InputPort(1).Data(3) = pi/2;
block.InputPort(1).Data(4) = 0;
block.InputPort(1).Data(5) = -pi/2;
block.InputPort(1).Data(6) = 0;
block.InputPort(1).Data(7) = 0;
block.InputPort(1).Data(8) = 0;
block.InputPort(1).Data(9) = 0;
block.InputPort(1).Data(10) = 0;
block.InputPort(1).Data(11) = 0;
block.InputPort(1).Data(12) = 0;
function Outputs(block)
gravityCompensation = 1; %1 if yes, 0 if no
g = 9.81;
%Defining input ports
u1 = block.InputPort(2).Data(1);
u2 = block.InputPort(2).Data(2);
u3 = block.InputPort(2).Data(3);
u4 = block.InputPort(2).Data(4);
u5 = block.InputPort(2).Data(5);
u6 = block.InputPort(2).Data(6);
q1 = block.InputPort(1).Data(1);
Dq1 = block.InputPort(1).Data(2);
q2 = block.InputPort(1).Data(3);
Dq2 = block.InputPort(1).Data(4);
q3 = block.InputPort(1).Data(5);
Dq3 = block.InputPort(1).Data(6);
q4 = block.InputPort(1).Data(7);
Dq4 = block.InputPort(1).Data(8);
q5 = block.InputPort(1).Data(9);
Dq5 = block.InputPort(1).Data(10);
q6 = block.InputPort(1).Data(11);
Dq6 = block.InputPort(1).Data(12);
%Calculating the elements of M(inertia matrix),C(coriolis terms),g(gravity) one by one
m11 = cos((q5 + 2 * pi - 2 * q4 - 2 * q3 - 2 * q2)) * 0.216840434497100888e-18 + cos((-2 * q2 + pi)) * 0.224527199999999994e1 + cos((2 * q4 + 2 * q3 + 2 * q2 + q5)) * 0.216840434497100888e-18 + cos((-2 * q4 + 2 * q3 + 2 * q2 + q5)) * 0.216840434497100888e-18 + sin(-q2 + pi / 0.2e1 - q3) * 0.333200000000000052e0 + 0.6800000000e-4 * cos((2 * q5 + 3 * pi + q4 - 2 * q3 - 2 * q2)) - 0.1700000000e-4 * cos((2 * q5 + pi + 2 * q3 + 2 * q2 - 2 * q4)) + cos((q5 + q4 + 2 * q3 + 2 * q2)) * 0.275499999999999964e-2 + cos((q5 + 2 * pi - q4 - 2 * q3 - 2 * q2)) * (-0.275499999999999964e-2) + sin((q5 + pi - q4 + q3)) * 0.260999999999999991e-2 + cos((q5 + 2 * q2 + 2 * q3 - q4)) * 0.275499999999999964e-2 + cos((pi + 2 * q4 + q5)) * (-0.433680868994201774e-18) - 0.6800000000e-4 * cos((-q4 + 2 * q5 + pi + 2 * q3 + 2 * q2)) + sin((q5 + 2 * pi - q4 - q3 - 2 * q2)) * 0.260999999999999991e-2 + sin((q5 + q4 + q3 + 2 * q2)) * 0.260999999999999991e-2 + cos((q5 + 2 * pi - 2 * q3 - 2 * q2)) * 0.551000000000000101e-2 + sin((q5 + pi + q3)) * 0.521999999999999982e-2 + sin((q5 + pi - q3)) * (-0.521999999999999982e-2) + cos(-q2 + pi / 0.2e1) * 0.192948000000000008e1 + cos((-2 * q2 + pi + 2 * q4)) * 0.138777878078144570e-16 + sin(q3) * (-0.856800000000000006e0) - 0.1700000000e-4 * cos((2 * q5 + 3 * pi - 2 * q3 - 2 * q2 + 2 * q4)) + 0.3400000000e-4 * cos((2 * q4 + 2 * pi + 2 * q5)) + cos((2 * q4 - 2 * q3 - 2 * q2 + pi)) * 0.340000000000062474e-4 + cos((q5 + 2 * pi + 2 * q4 - 2 * q3 - 2 * q2)) * 0.216840434497100888e-18 + sin((-q3 - 2 * q2 + pi)) * 0.856800000000000006e0 - 0.6800000000e-4 * cos((q4 + 2 * q5 + pi + 2 * q3 + 2 * q2)) + cos((q5 + pi)) * (-0.110200000000000020e-1) + cos((q5 + 2 * q2 + 2 * q3)) * 0.551000000000000101e-2 + sin(q5 + 0.3e1 / 0.2e1 * pi - q3 - q2) * (-0.203000000000000012e-2) + sin((q5 + pi + q4 - q3)) * 0.260999999999999991e-2 + sin((q5 + pi + q4 + q3)) * 0.260999999999999991e-2 + sin((q5 + pi - q4 - q3)) * 0.260999999999999991e-2 + cos((pi - 2 * q4 + q5)) * (-0.433680868994201774e-18) - 0.1700000000e-4 * cos((2 * q5 + 3 * pi - 2 * q4 - 2 * q3 - 2 * q2)) + 0.6800000000e-4 * cos((2 * q5 + 3 * pi - q4 - 2 * q3 - 2 * q2)) + sin((q5 + 2 * pi + q4 - q3 - 2 * q2)) * 0.260999999999999991e-2 + sin((q5 - q4 + q3 + 2 * q2)) * 0.260999999999999991e-2 - 0.6800000000e-4 * cos((2 * pi + 2 * q5)) + cos((-2 * q2 + pi - 2 * q3)) * (-0.499655500000000030e0) + 0.3400000000e-4 * cos((-2 * q4 + 2 * pi + 2 * q5)) - 0.1020000000e-3 * cos((2 * q5 + 3 * pi - 2 * q3 - 2 * q2)) - 0.1020000000e-3 * cos((2 * q5 + pi + 2 * q3 + 2 * q2)) + sin(q5 + pi / 0.2e1 + q3 + q2) * 0.203000000000000012e-2 + cos((q5 + 2 * pi - 2 * q2 - 2 * q3 + q4)) * (-0.275499999999999964e-2) - 0.1700000000e-4 * cos((2 * q5 + pi + 2 * q4 + 2 * q3 + 2 * q2)) + cos((2 * q4)) * (-0.679999999999847394e-4) + cos((-2 * q4 - 2 * q3 - 2 * q2 + pi)) * 0.340000000000062474e-4 + sin((q5 + 2 * pi - q3 - 2 * q2)) * (-0.521999999999999982e-2) + sin((q5 + q3 + 2 * q2)) * 0.521999999999999982e-2 + 0.374662799999999940e1 + sin(q5 + pi / 0.2e1 + q3 + q2 + q4) * 0.101500000000000006e-2 + sin(q5 + 0.3e1 / 0.2e1 * pi - q3 - q2 + q4) * 0.101500000000000006e-2 + sin(q5 + pi / 0.2e1 + q3 + q2 - q4) * 0.101500000000000006e-2 + sin(q5 + 0.3e1 / 0.2e1 * pi - q3 - q2 - q4) * 0.101500000000000006e-2 + cos((-2 * q2 + pi - 2 * q4)) * 0.138777878078144570e-16;
m12 = cos(0.3e1 / 0.2e1 * pi - (2 * q4) - q3 - q2 + q5) * 0.433680868994201774e-18 + cos(q5 + 0.3e1 / 0.2e1 * pi + (2 * q4) - q3 - q2) * (-0.433680868994201774e-18) + 0.6800000000e-4 * cos(-q4 + 0.5e1 / 0.2e1 * pi + 0.2e1 * q5 - q3 - q2) - 0.2755000000e-2 * cos(q5 + 0.3e1 / 0.2e1 * pi - q3 - q2 - q4) + cos(q5 + pi / 0.2e1 - (2 * q4) + q3 + q2) * (-0.433680868994201774e-18) + 0.2610000000e-2 * sin(q5 + pi / 0.2e1 + q4 + q2) + cos(pi / 0.2e1 + (2 * q4) + q3 + q2 + q5) * 0.433680868994201774e-18 - 0.2610000000e-2 * sin(q5 + pi / 0.2e1 - q4 + q2) + 0.2755000000e-2 * cos(q5 + pi / 0.2e1 + q3 + q2 + q4) - 0.3400000000e-4 * cos((2 * q4) + 0.2e1 * q5 + 0.3e1 / 0.2e1 * pi + q3 + q2) + 0.2610000000e-2 * sin(q5 + 0.3e1 / 0.2e1 * pi - q4 - q2) + 0.3400000000e-4 * cos(-(2 * q4) + 0.2e1 * q5 + 0.3e1 / 0.2e1 * pi + q3 + q2) + cos((2 * q4) - q3 - q2 + pi / 0.2e1) * (-0.680000000000402504e-4) + cos(-q2 + pi / 0.2e1 - (2 * q4) + q3) * (-0.277555756156289136e-16) + 0.3400000000e-4 * cos((2 * q4) + 0.2e1 * q5 + 0.5e1 / 0.2e1 * pi - q3 - q2) + cos(-(2 * q4) - q3 - q2 + pi / 0.2e1) * 0.680000000000124950e-4 + 0.309540e0 * sin(-q2 + pi / 0.2e1) - 0.2610000000e-2 * sin(q5 + 0.3e1 / 0.2e1 * pi + q4 - q2) - 0.6800000000e-4 * cos(0.2e1 * q5 + 0.3e1 / 0.2e1 * pi + q4 + q3 + q2) - 0.6800000000e-4 * cos(0.2e1 * q5 + 0.5e1 / 0.2e1 * pi + q4 - q3 - q2) + 0.6800000000e-4 * cos(0.2e1 * q5 + 0.3e1 / 0.2e1 * pi - q4 + q3 + q2) - 0.3400000000e-4 * cos(-(2 * q4) + 0.2e1 * q5 + 0.5e1 / 0.2e1 * pi - q3 - q2) - 0.2755000000e-2 * cos(q5 + pi / 0.2e1 + q3 + q2 - q4) + 0.2755000000e-2 * cos(q5 + 0.3e1 / 0.2e1 * pi - q3 - q2 + q4);
m13 = 0.6800000000e-4 * cos(-q4 + 0.5e1 / 0.2e1 * pi + (2 * q5) - q3 - q2) + 0.6800000000e-4 * cos((2 * q5) + 0.3e1 / 0.2e1 * pi - q4 + q3 + q2) - 0.34000
