PD控制.rar

%S-function for continuous state equation function [sys,x0,str,ts]=s_function(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 p g sizes = simsizes; sizes.NumContStates = 4; sizes.NumDiscStates = 0; sizes.NumOutputs = 4; sizes.NumInputs =2; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 0; sys=simsizes(sizes); x0=[0 0 0 0]; str=[]; ts=[]; p=[2.9 0.76 0.87 3.04 0.87]; g=9.8; function sys=mdlDerivatives(t,x,u) global p g D0=[p(1)+p(2)+2*p(3)*cos(x(3)) p(2)+p(3)*cos(x(3)); p(2)+p(3)*cos(x(3)) p(2)]; C0=[-p(3)*x(4)*sin(x(3)) -p(3)*(x(2)+x(4))*sin(x(3)); p(3)*x(2)*sin(x(3)) 0]; tol=u(1:2); dq=[x(2);x(4)]; S=inv(D0)*(tol-C0*dq); sys(1)=x(2); sys(2)=S(1); sys(3)=x(4); sys(4)=S(2); function sys=mdlOutputs(t,x,u) sys(1)=x(1); sys(2)=x(2); sys(3)=x(3); sys(4)=x(4);