%S-function for continuous state equation
function [sys,x0,str,ts]=chap2_1plant(t,x,u,flag)
switch flag
%Initialization
case 0
[sys,x0,str,ts]=mdlInitializeSizes;
case 1
sys=mdlDerivatives(t,x,u);
%Outputs
case 3
sys=mdlOutputs(t,x,u);
%Unhandled flags
case {2, 4, 9 }
sys = [];
%Unexpected flags
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end
%mdlInitializeSizes
function [sys,x0,str,ts]=mdlInitializeSizes
global g
sizes = simsizes;
sizes.NumContStates = 2;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 2;
sizes.NumInputs =2;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 0;
sys=simsizes(sizes);
x0=[0 0 0 0];
str=[];
ts=[];
g=9.81;
function sys=mdlDerivatives(t,x,u)
global g
m1=10;m2=5;L1=0.7;L2=0.5;
qm_1=x(1);
qm_2=x(2);
dqm_1=x(3);
dqm_2=x(4);
qm=[qm_1,qm_2];
dqm=[dqm_1;dqm_2];
Mm=[m1*0.25*L1^2+(1/12)*m1*L1^2+m2(L1^2+0.25*L^2+2*L1*L2*cos(qm_2))+(1/12)*m1*L2^2 m2(0.25*L1^2+L1*L2*cos(qm_2))+(1/12)*m1*L2^2;m2(0.25*L1^2+L1*L2*cos(qm_2))+(1/12)*m1*L2^2 m2*0.25*L2^2+(1/12)*m1*L2^2];
Cm=[-m2*L1*0.5*L2*(dqm_2+2*dqm_1*dqm_2)*sin(qm_2);m2*L1*0.5*L2*sin(qm_2)];
gm=[m1*g*0.5*L2*cos(qm_1)+m2*g(L1*cos(qm_1)+0.5*L2*cos((qm_1)+(qm_2)));m2*g*0.5*L2*cos((qm_1)+(qm_2))]
tol=u(1:2);
S=inv(Mm)*(tol-Fh-Cm*dqm-gm);
Fh=Jm'*[0 1]'*F;
Jm=[-0.5*L1*sin(qm_1) 0;0.5*L1*cos(qm_1) 0;0 0];
t=0:0.1:10;
F=(3*t).*(t>=0 & t<=10)+30.*(t>10 & t<40)+(-3*t+150).*(t>=40 & t<=50)+0.*(t>50);
sys(1)=x(1);
sys(2)=S(1);
sys(3)=x(2);
sys(4)=S(2);
function sys=mdlOutputs(t,x,u)
sys(1)=x(1);
sys(2)=x(2);
机械臂PD控制matlab,机械臂plc控制,matlab
版权申诉
5星 · 超过95%的资源 36 浏览量
2021-09-10
15:11:21
上传
评论 5
收藏 55KB ZIP 举报
lithops7
- 粉丝: 325
- 资源: 4457
- 1
- 2
- 3
前往页