robot.m

利用simulink简历模块图进行机器人的单关节动力学仿真

function [sys,x0,str,ts] = sfun_direct_F(t,x,u,flag)

switch flag,
 case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;
 case 1,
    sys=mdlDerivatives(t,x,u);
 case 2,
    sys=mdlUpdate(t,x,u);
 case 3,
    sys=mdlOutputs(t,x,u);
 case 4,
    sys=mdlGetTimeOfNextVarHit(t,x,u);
 case 9,
    sys=mdlTerminate(t,x,u);
    otherwise
    error(['Unhandled flag = ',num2str(flag)]);

end

function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;

sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 5;
sizes.NumInputs      = 8;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;  

sys = simsizes(sizes);
x0  = [];
str = [];
ts  = [0 0];

function sys=mdlDerivatives(t,x,u)

sys = [];
function sys=mdlUpdate(t,x,u)

sys = [];
function sys=mdlOutputs(t,x,u)
% u(1)=omega-1
% u(2)=theta-1
% u(3)=omega-2
% u(4)=theta-2
% u(5)=torque-1
% u(6)=torque-2
% u(7)=pl-x
% u(8)=pl-y

% define some local variables
r1=1.0; r2=0.8;   % length of connecting rod 1 & 2
rc1=0.5; rc2=0.1; % length between joint and COM
m1=2.5; m2=1.8;   % m1,m2: MASS of connecting rod 1 & 2
i1=0.15; i2=0.05; % moment of inertia to COM
g=9.8067;    % in kg/s^2

mpl=2.0; % MASS of terminal manipulator

s1=sin(u(2)); s12=sin(u(2)+u(4));
c1=cos(u(2)); c12=cos(u(2)+u(4));

a=zeros(14,14);
b=zeros(14,1);

a(1,1)=r1*s1+r2*s12;  a(1,2)=r2*s12;  a(1,7)=1;
a(2,1)=-r1*c1-r2*c12;  a(2,2)=-r2*c12;  a(2,8)=1;
a(3,1)=rc1*s1;  a(3,3)=1;
a(4,1)=-rc1*c1;  a(4,4)=1;
a(5,1)=r1*s1+rc2*s12;  a(5,2)=rc2*s12;  a(5,5)=1;
a(6,1)=-r1*c1-rc2*c12;  a(6,2)=-rc2*c12;  a(6,6)=1;
a(7,3)=-m1;  a(7,9)=1;  a(7,11)=1;
a(8,4)=-m1;  a(8,10)=1;  a(8,12)=1;
a(9,1)=i1;  a(9,11)=r1*s1;  a(9,12)=-r1*c1;
a(10,5)=-m2;  a(10,11)=-1;  a(10,13)=1;
a(11,6)=-m2;  a(11,12)=-1;  a(11,14)=1;
a(12,2)=i2;  a(12,11)=rc2*s12;  a(12,12)=-rc2*c12;  a(12,13)=(r2-rc2)*s12;  a(12,14)=-(r2-rc2)*c12;
a(13,7)=mpl;  a(13,13)=1;
a(14,8)=mpl;  a(14,14)=1;

b(1,1)=-((r1*c1+r2*c12)*u(1)^2+r2*c12*u(3)^2+2*r2*u(1)*u(3)*c12);
b(2,1)=-((r1*s1+r2*s12)*u(1)^2+r2*s12*u(3)^2+2*r2*u(1)*u(3)*s12);
b(3,1)=-rc1*c1*u(1)^2;
b(4,1)=-rc1*s1*u(1)^2;
b(5,1)=-((r1*c1+rc2*c12)*u(1)^2+rc2*c12*u(3)^2+2*rc2*u(1)*u(3)*c12);
b(6,1)=-((r1*s1+rc2*s12)*u(1)^2+rc2*s12*u(3)^2+2*rc2*u(1)*u(3)*s12);
b(8,1)=m1*g;
b(9,1)=u(5)-u(6)-m1*g*rc1*c1;
b(11,1)=m2*g;
b(12,1)=u(6);
b(14,1)=-mpl*g;

% solve the equations
fff=inv(a)*b;
%compute the consistency error
error=norm([u(7)-r1*c1-r2*c12,u(8)-r1*s1-r2*s12]);

%output the result
outfff(1)=fff(1);
outfff(2)=fff(2);
outfff(3)=fff(7);
outfff(4)=fff(8);
outfff(5)=error;
sys=outfff;

function sys=mdlGetTimeOfNextVarHit(t,x,u)

sampleTime = 1;   
sys = t + sampleTime;

function sys=mdlTerminate(t,x,u)

sys = [];