%yaw control of a mariner type vessel (msstoolbox fossen, perez)
%c.d.
clear all
clc

tf = 500;
h = 0.1;
T=0:h:tf;
tun_rule=dread('Choose 0 for \delta PD-tuning or 1 for \delta PID');
K=0.0015; tau=0.0186; %generated from delta-opt-method (dalen-ruscio-2017)
delta=8; %tuning

% PID controller: delta-tuning
[Kp,Td]=pd_tun_maxdelay(K,tau,delta,2.24);
if tun_rule==0
    gamma=inf;
else
    gamma=2.24;
end
Ti=gamma*Td;

x = zeros(7,1);   

N = round(tf/h);
X = zeros(N+1,8);
z=0;
KpTih=Kp*h/Ti;r0=10;
r = r0*(pi/180); 
v = 25*(pi/180); 
for i=1:N+1
    y=x(6);
    e=r-y;
    u = Kp*e-Kp*Td*x(3)+z;
    z=z+KpTih*e;
    
    % input dist. after 250 sec
    if i>2500
       u=u+v;  
    end
    
    [f,U] = mariner(x,u,15.43);%nominal speed set to 15.43 m/s <--> 30 knot
    X(i,:) = [x',U]; 
    
    x = x + h*f;
end

figure(1)
plot(X(:,5),X(:,4)),grid,axis('equal'),xlabel('East'),ylabel('North'),title('Ship position')

figure(2)
subplot(211),plot(T,X(:,6)*180/pi,T,r0*ones(length(T),1),'r--'),xlabel('time (s)'),title('yaw angle (deg)'),grid
subplot(212),plot(T,X(:,7)*180/pi),xlabel('time (s)'),title('rudder angle (deg)'),grid

figure(3)
plot(T,X(:,8)),xlabel('time (s)'),title('%nominal speed set to 15.43 m/s'),grid

