%CINEMATICADIRECTA Sfunction it is used to compute accelerations and velocities of a
% manipulator.
%
% This Sfunción calculates the robot acceleration joints.The block entry is
% the u vector whose components are positions and velocities of articular
% coordinates [q qd] and the robot object called RRed.
%
% [sys,x0,str,ts] =CINDIR(t,x,u,flag,RRed)
%
% RRed is an naxis robot object and describes the manipulator dynamics and
% kinematics
%
% Implementation in Simulink.
%
% See also: ACCEL_H,ACCEL, ROBOT, ODE45.
% 2007 Niceto Luque Sola
function [sys,x0,str,ts] =CINDIR(t,x,u,flag,RRed)
switch flag,
case 0
% initialize robot dimensions
[sys,x0,str,ts] = mdlInitializeSizes(RRed); % Init
case {3}
% come here to calculate derivitives
q=[u(1:RRed.n)];% articular postitions vector
qd=[u(RRed.n+1:2*RRed.n)];%articular velocities vector
tau=[u(2*RRed.n+1:end)]; % external applied torque
sys = accel(RRed,q,qd,tau);%inverse dynamic
%sys=accel(RRed,u);
case {1, 2, 4, 9}
sys = [];
end
%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the Sfunction.
%=============================================================================
%
function [sys,x0,str,ts]=mdlInitializeSizes(RRed)
%
% 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 Sfunction parameters.
%
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 0;
sizes.NumOutputs = RRed.n;
sizes.NumInputs = 3*RRed.n;
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 = [0 0];
% end mdlInitializeSizes
