www.pudn.com > SHIPCONTROL.rar > PIDshipmotioncontrol.m, change:2015-12-02,size:1086b


 
clear all;clc; 
 
 
Kp=0.9; 
 Td=0; 
 Ti=400; 
 
 
 buchang=1; 
  
n=1500; 
  
sdd=0; 
ui=zeros(1,n); 
m7=7; 
YY=zeros(n+1,m7); 
YY01=zeros(1,n); 
YY(1,:)=[1 0 0 0.5 0 0 0]; 
for i=2:n 
 gt=mariner(YY(i,:),ui(i)); 
   for jv=1:m7 
       if i>1 
    YY(i+1,jv)=YY(i,jv)+gt(jv); 
       end 
   end 
x01=YY(i,5); 
y01=YY(i,6); 
delta=YY(i,7); 
if i>2 
x00(i)=gt(5); 
 
y00(i)=gt(6); 
YY01(i)=atan2(y00(i),x00(i));    
 
 
plot(x01,y01); 
 
pause(0.001); 
hold on 
 
 
end 
 
if i>2 
 
end  
 
end 
YY01 
figure 
plot(YY01); 
figure 
plot(YY(:,4)) 
figure 
 plot(YY(:,7)) 
 figure 
 plot(ui) 
% if ssdr==1 
%     break 
% end 
% end 
 
% [hang,lie]=size(x); 
% x0=zeros(1,hang); 
% y0=zeros(1,hang); 
% bosai0=zeros(1,hang); 
% FAI0=zeros(1,hang); 
% for i=2:hang-1 
%     x0(i+1)=(u(i)*cos(bosai(i))-v(i)*cos(FAI(i))*sin(bosai(i)))*(time(i)-time(i-1))+x0(i); 
%     y0(i+1)=(u(i)*sin(bosai(i))+v(i)*cos(FAI(i))*cos(bosai(i)))*(time(i)-time(i-1))+y0(i); 
%     bosai0(i+1)=(r(i)*cos(FAI(i)))*(time(i)-time(i-1))+bosai0(i); 
%     FAI0(i+1)=p(i)*(time(i)-time(i-1))+FAI0(i); 
% end