www.pudn.com > AUV-tarcking.zip > control.m, change:2012-04-25,size:1302b


function [sys,x0,str,ts] = s_function(t,x,u,flag) 
 
switch flag, 
  case 0, 
    [sys,x0,str,ts]=mdlInitializeSizes; 
 
  case 3, 
    sys=mdlOutputs(t,x,u); 
 
  case {2,4,9} 
    sys=[]; 
     
  otherwise 
    error(['Unhandled flag = ',num2str(flag)]); 
 
end 
 
function [sys,x0,str,ts]=mdlInitializeSizes 
 
sizes = simsizes; 
 
sizes.NumOutputs     = 3; 
sizes.NumInputs      = 9; 
sizes.DirFeedthrough = 1; 
sizes.NumSampleTimes = 1;    
 
sys = simsizes(sizes); 
 
x0  = []; 
str = []; 
ts  = [0 0]; 
 
function sys=mdlOutputs(t,x,u) 
 
 
L=1.46; m=45;density=1025.0; 
L2=0.5*density*L*L; 
L3=L2*L; 
L4=L3*L; 
L5=L4*L; 
Xdu=-1.5777e-3*L3;Xuu=-5.9e-3*L2;Xu=0; 
Ydv=-3.0753e-2*L3;Yv=-4.4961e-2*L2;Yvv=-1.6687e-1*L2; 
Iz=8.067;Ndr=-1.6012e-3*L5;Nrr=-1.4432e-3*L5;Nr=-1.1679e-2*L4; 
 
m11=m-Xdu;m22=m-Ydv;m33=Iz-Ndr;d11=-Xu-Xuu*norm(u(1));d22=-Yv-Yvv*norm(u(2)); 
d33=Nr-Nrr*norm(u(3)); 
Ud=u(4); 
Ue=u(1)-Ud; 
dUd=u(5); 
Ku=0.5; 
X=m11*(-Ku*Ue+dUd)-m22*u(2)*u(3)+d11*u(1); 
dFieB=u(6); 
FieB=u(7); 
ddBeta=u(8); 
ddFied=u(9); 
 
 
Kr=0.5; 
Kf=5; 
N=m33*(-Kr*dFieB-Kf*FieB-ddBeta+ddFied)-(m11-m22)*u(1)*u(2)+d33*u(3); 
 
if X>30 
    sys(1)=30; 
else  
    sys(1)=X; 
end 
 
sys(2)=0; 
 
if N>20 
    sys(3)=20; 
elseif N<-20 
    sys(3)=-20; 
else 
    sys(3)=N; 
end