%clear all clear close all global PIDValues PIDValues = zeros(1,7); yold = 0; TC_P = 0; TC_I = 0; TC_D = 0; dt = 0.05; %timestep size tf = 100; N = tf/dt; %Number of timesteps to compute %Set up "Buoy" Buoy = RigidBody; Buoy = set(Buoy,'name','Surface Buoy'); Buoy = set(Buoy,'dt',dt,'N',N); %Set timesteps Buoy = set(Buoy,'dof',6); %With 6 DOF. Buoy = set(Buoy,'Fixed',0); %Set free to move espRadius = 25.5 * 0.0254 / 2.0; %meters (25.5" in diameter) espMass = 620; %from SW in kg espWPArea = pi * espRadius^2; espWtH2O = 3450; %N espDisp = ((espMass * 9.8) - espWtH2O) / (1025.0 * 9.8); espLength = espDisp / (pi * espRadius^2); espPJASubmergedX = espRadius * 2.0 * espLength; espPJASubmergedY = espPJASubmergedX; espPJASubmergedZ = espWPArea; buoyRadius = 66.0 * 0.0254 / 2.0; %meters (66" in diameter) buoyMass = 691.0; %from SW in kg buoyWPArea = pi * buoyRadius^2; buoyDisp = ((buoyMass + espMass) / 1025.0) - espDisp; buoyWPA2ndMoment = (pi * buoyRadius^4) / 4; %(pi*r^4)/4 for a circle buoyPJASubmergedX = buoyRadius * 2.0 * 0.686; buoyPJASubmergedY = buoyPJASubmergedX; buoyPJASubmergedZ = buoyWPArea; %VVVVVVVVV Change this code to reflect ESP Drifter Buoy characteristics VVVVVVVV Buoy = set(Buoy,'mass', buoyMass,'S', buoyWPArea,'disp', buoyDisp,'g',9.80665); %GM mass, waterplane area, displacement, g Buoy = set(Buoy,'x0',[0 0 0]); %GM Equilibrium Position in global coordinates. Buoy = set(Buoy,'xG',[0 0 -0.2]); %GM Center of mass X, Y, Z actual values from SW are X-0.01 Y0.08 Z0.00 Buoy = set(Buoy,'xB',[0 0 -0.19]); %GM Center of buoyancy X, Y, Z actual values from SW are X-0.00 Y-0.19 Z0.00 Buoy = set(Buoy,'S11',buoyWPA2ndMoment,'S22', buoyWPA2ndMoment); %GM 2nd moment of area of Waterplane in x (S11) and Y (S22) (pi*r^4)/4 for a cricle I = zeros(3); I(1,1) = 319.9; I(2,2) = 343.4; I(3,3) = 122.0; %GM SW moment of inertia about body's coordinated system Buoy = set(Buoy,'I',I); clear I; Buoy = set(Buoy,'Area',[buoyPJASubmergedX buoyPJASubmergedY buoyPJASubmergedZ]); %GM projected area of submerged parts in X, Y, Z Buoy = set(Buoy,'Cd',[50 1 0]); %GM Cds for submerged part leave these alone Buoy = set(Buoy,'AreaW',[1 1 0]); %GM Area of super surface elements for wind drag Buoy = set(Buoy,'CdW',[1 1 0]); % GM Cds for wind %************************************************************************** %Read Frequency Domain Hydrodynamic Data from file. %******** Change this filename to be the WAMIT results for the ESP drifter* filename = 'WAMIT_Results\ESP_drifter'; %filename = 'test25a'; %************************************************************************** disp('Reading frequency domain hydrodynamic coefficients'); FD_coefs = ReadWamitFD(filename); for i = 1:6 for j = 1:6 mu_inf(i,j) = 1025*FD_coefs.X{i,j}(2); end end Buoy = set(Buoy,'mu_inf',mu_inf); clear mu_inf; %Compute Impulse Response Functions from Frequency Domain Results disp('Computing impulse response functions'); L = ComputeWamitIRF_coefs(filename); %Set up "ESP" ESP = RigidBody; ESP = set(ESP,'name','ESP'); ESP = set(ESP,'dt',dt,'N',N); %Set timesteps ESP = set(ESP,'dof',6); %With 6 DOF. ESP = set(ESP,'Fixed',0); %Set free to move %VVVVVVVVVVV Change this code to reflect ESP characteristics **************** ESP = set(ESP,'x0',[0 0 -10]); %Equilibrium Position in global coordinates. ESP = set(ESP,'mass', espMass,'S', 0,'disp', espDisp,'g',9.81);% GM ESP = set(ESP,'xG',[0 0 -1.03]); % GM ESP = set(ESP,'xB',[0 0 -.75]); % GM ESP = set(ESP,'S11',0.0,'S22',0.0); % GM I = zeros(3); I(1,1) = 551.5; I(2,2) = 551.6; I(3,3) = 27.9; % GM ESP = set(ESP,'I',I); clear I; ESP = set(ESP,'Area',[espPJASubmergedX espPJASubmergedY espPJASubmergedZ]); % GM ESP = set(ESP,'Cd',[1.5 1.5 1.0]); % GM ro = get(Buoy,'rho'); %kg /m^3 b = espLength / 2.0; a = espRadius; espAMX = 3 * 3.14 * ro * b^2 * (espRadius * 2.0) / 4.0; espAMY = 3 * 3.14 * ro * a^2 * (espRadius * 2.0) / 4.0; mu_inf = zeros(6,6); mu_inf(1,1) = espAMX; %GM added mass in surge horizontal mu_inf(2,2) = espAMX; %GM added mass in surge horizontal mu_inf(3,3) = espAMY; %GM added mass heave Z mu_inf(5,5) = 0.0; %GM added mass pitch %************************************************************************** ESP = set(ESP,'mu_inf',mu_inf); clear mu_inf; %Compute incident wave profile Type = 0; %GM Type refers monochromatic(Type = 1) or PM spectrum (Type = 0) wave T = 8.0; %GM wave period or peak wave period in seconds A = 1.5; %GM Wave amplitude in meters wave = IncWave; wave = set(wave,'T',T); wave = set(wave,'A',A); wave = set(wave,'Type',Type); wave = set(wave,'beta',-1); disp('Computing incident wave profile'); t = get(Buoy,'t'); t1= dt:dt:L{7,1}.breaks(end); eta0 = eta(wave,0,[t t(end)+t1]); %Add forces to Buoy Buoy = add_Force_fcn(Buoy,@F_HydroStatic); Buoy = add_Force_fcn(Buoy,@F_Weight); %Buoy = add_Force_fcn(Buoy,@(body,RB,n) Mem_Radiation(body,RB,n,L)); Buoy = add_Force_fcn(Buoy,@(body,RB,n) F_Exciting(body,RB,n,L,eta0)); %Buoy = add_Force_fcn(Buoy,@(body,RB,n) F_Drag_Inc(body,RB,n,'Surface Buoy',wave)); %Add forces to ESP ESP = add_Force_fcn(ESP,@F_HydroStatic); ESP = add_Force_fcn(ESP,@F_Weight); ESP = add_Force_fcn(ESP,@(body,RB,n) F_Drag_Inc(body,RB,n,'Surface Buoy',0)); %Add forces that act on two bodies %******* Change this code to reflect Buoy and ESP attachement points ****** p_aB = [0.0 0.0 -0.5]; %GM Tether attachment point on Buoy (local CoordSys). p_aE = [0.0 0.0 0.0]; %GM Tether attachment point on ESP (local CoordSys). %************************************************************************** if(1==0) %****** Use this code to include ESP tension control algorithm (comment out section below) ************ Buoy = add_Force_fcn(Buoy,@(body,RB,n) WinchControlledTether(body,RB,n,'ESP',p_aB,p_aE)); ESP = add_Force_fcn(ESP,@(body,RB,n) WinchControlledTether(body,RB,n,'Surface Buoy',p_aE,p_aB)); %************************************************************************** else %****** Use this code to model fixed cable length (comment out add_forces above) ************ k = 100000; %N/m [Spring_Force_Coeffs] = LinearSpringForce(k); % b = 100; %N/(m/s) [Damping_Force_Coeffs] = LinearDampingForce(b); % Buoy = add_Force_fcn(Buoy,@(body,RB,n) Hydraulic_tether(body,RB,n,'ESP',p_aB,p_aE,Spring_Force_Coeffs,Damping_Force_Coeffs)); ESP = add_Force_fcn(ESP,@(body,RB,n) Hydraulic_tether(body,RB,n,'Surface Buoy',p_aE,p_aB,Spring_Force_Coeffs,Damping_Force_Coeffs)); %************************************************************************** end %Set Initial Conditions %Offset position from equilibrium position. Buoy = set_pos(Buoy,[0 0 0 0 0 0],1); ESP = set_pos(ESP,[0 0 (-espWtH2O / k) 0 0 0],1); RB{1} = Buoy; RB{2} = ESP; %Solve for motion RB = Solve_RK4(RB); Buoy = RB{1}; ESP = RB{2}; %Plot Buoy motion results x = get(Buoy,'x'); xdot = get(Buoy,'xdot'); xddot = get(Buoy,'xddot'); v1 = xdot(:,3); %Plot ESP motion results y = get(ESP,'x'); ydot = get(ESP,'xdot'); yddot = get(ESP,'xddot'); v2 = ydot(:,3); %plotting results Bforce = get(Buoy,'Forces'); ESPForce = get(ESP,'Forces'); xo1 = get(ESP,'x0'); xo2 = get(Buoy,'x0'); for i = 1:N [TargetDepth(i) TargetDepthRate(i)] = TargetDepth_Profiling(t(i)); [Elongation(i),ElongationRate(i),s] = TetherElongations(xo1,y(i,:)',ydot(i,:)',p_aE,xo2,x(i,:)',xdot(i,:)',p_aB); Tension(i) = norm(ESPForce{end}(i,1:3)); end %Buoy Motions figure('Units','normalized','Position', [.05 .55 .4 .4]) set(gcf,'color','white'); subplot(2,1,1),plot(t,x(:,3),t,x(:,5)); title('Buoy Motions ','FontName','times','FontSize', 12); legend('heave(m)','pitch(rad)'); subplot(2,1,2),plot(t,xdot(:,3),t,xdot(:,5)); legend('heave vel (m/s)','pitch vel (m/s)'); xlabel('time (s)'); %ESP Motions %figure('Position', [100 100 1000 600]) figure('Units','normalized','Position', [.05 .05 .4 .4]) set(gcf,'color','white'); subplot(2,1,1), plot(t,y(:,3),t,y(:,5),t,TargetDepth); title('ESP Motions','FontName','times','FontSize', 12); legend('heave (m)','pitch (rad)','target depth (m)'); subplot(2,1,2), plot(t,ydot(:,3),t,ydot(:,5)); legend('heave vel (m/s)','pitch vel (rad/s)'); xlabel('time (s)'); %ESP Position/Velocity Error %figure('Position', [100 100 1000 600]) figure('Units','normalized','Position', [.55 .55 .4 .4]) set(gcf,'color','white'); subplot(2,1,1) plot(t,y(:,3)-TargetDepth'); title('ESP Depth Error','FontName','times','FontSize', 12); ylabel('Depth Error (m)'); subplot(2,1,2) plot(t,ydot(:,3)-TargetDepthRate'); title('ESP Depth Rate Error','FontName','times','FontSize', 12); ylabel('Velocity Error (m/s)'); xlabel('time (s)'); %Forces and Power Power = -ElongationRate.*Tension; idx = find(ElongationRate < 0); %Find when moving up. mean_power = mean(Power); %mean_power_noregen = mean(Power(idx)); %figure('Position', [100 100 1000 600]) figure('Units','normalized','Position', [.55 .05 .4 .4]) set(gcf,'color','white'); subplot(3,1,1) plot(t,ElongationRate) legend('Payout Rate (m/s)'); ylabel('m/s'); % titleText = sprintf('Tension Control k: %5.1f b: %5.1f ESP Weight in H2O: %5.1f', gTC_k, gTC_b, espWtH2O); % title(titleText); subplot(3,1,2) plot(t,Tension/4.45) legend('Tether Tension (lbs)'); ylabel('lbs'); subplot(3,1,3) plot(PIDValues(:,6)) % plot(t,Power,t(1:length(t)-1:end),mean_power*ones(1,2),t(1:length(t)-1:end),mean_power_noregen*ones(1,2)); % legend('Power (W)','Mean Power (with regen)','Mean Power (without regen)'); % ylabel('W'); % xlabel('time (s)'); figure('Units','normalized','Position', [.25 .05 .6 .25]) plot(t,Tension/4.45) title(sprintf('Cable Tension with A (Hs/2) = %5.2d and ESP Wt in H20 = %4.0f N b = %5.3f', A, espWtH2O, b)) ylabel('Tension (lbf)'); %axis([0 100 0 1000]); grid