You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
73 lines
1.9 KiB
73 lines
1.9 KiB
%SLACCEL S-function for robot acceleration
|
|
%
|
|
% This is the S-function for computing robot acceleration. It assumes input
|
|
% data u to be the vector [q qd tau].
|
|
%
|
|
% Implemented as an S-function to get around vector sizing problem with
|
|
% Simulink 4.
|
|
|
|
function [sys, x0, str, ts] = slaccel(t, x, u, flag, robot)
|
|
switch flag,
|
|
|
|
case 0
|
|
% initialize the robot graphics
|
|
[sys,x0,str,ts] = mdlInitializeSizes(robot); % Init
|
|
|
|
case {3}
|
|
% come here to calculate derivitives
|
|
|
|
% first check that the torque vector is sensible
|
|
if length(u) ~= (3*robot.n)
|
|
error('RTB:slaccel:badarg', 'Input vector is length %d, should be %d', length(u), 3*robot.n);
|
|
end
|
|
if ~isreal(u)
|
|
error('RTB:slaccel:badarg', 'Input vector is complex, should be real');
|
|
end
|
|
|
|
sys = robot.accel(u(:)');
|
|
case {1, 2, 4, 9}
|
|
sys = [];
|
|
end
|
|
%
|
|
%=============================================================================
|
|
% mdlInitializeSizes
|
|
% Return the sizes, initial conditions, and sample times for the S-function.
|
|
%=============================================================================
|
|
%
|
|
function [sys,x0,str,ts]=mdlInitializeSizes(robot)
|
|
|
|
%
|
|
% 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 = robot.n;
|
|
sizes.NumInputs = 3*robot.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
|