Control optimal gains interpolation (#96) (#106)

akeeste · MShabara · jtgrasb · web-flow · commit 21a305b5ee41 · 2026-03-05T10:14:22.000-06:00
* modifies the optimalGainCalc for the passive case

* modifies the optimalGainCalc for the reactive case

* Update optimalGainCalc.m

Adds semicolon to one of the lines

* modified the latching control gain calc

* corrects the function description

* modified the declutching control gain calc

* update frequency calculation

* revert period

* final cleanup

---------

Co-authored-by: Mohamed Shabara &lt;84589678+MShabara@users.noreply.github.com&gt;
Co-authored-by: jtgrasb &lt;87095491+jtgrasb@users.noreply.github.com&gt;
diff --git a/Controls/Declutching/optimalTimeCalc.m b/Controls/Declutching/optimalTimeCalc.m
@@ -1,14 +1,14 @@
 % This script identifies the dynamics of the float in the respective wave 
 % conditions and determines the optimal proportional gain value for a 
 % passive controller (for regular waves)
-
 close all; clear all; clc;
 
+dof = 3;            % Caluclate for heave motion
 % Inputs (from wecSimInputFile)
 simu = simulationClass();
-body(1) = bodyClass('../hydroData/sphere.h5');
-waves.height = 1;
-waves.period = 3.5;
+body(1) = bodyClass('../../_Common_Input_Files/Sphere/hydroData/sphere.h5');
+waves.height = 2.5;                          % Wave Height [m]
+waves.period = 9.6664;                       % Wave Period [s]
 
 % Load hydrodynamic data for float from BEM
 hydro = readBEMIOH5(body.h5File{1}, 1, body.meanDrift);
@@ -19,53 +19,62 @@
 T = waves.period;
 omega = (1/T)*(2*pi);
 
+% Extend the freq vector to include the wave frequency
+hydro.simulation_parameters.w_extended = sort([hydro.simulation_parameters.w omega]);
+omegaIndex = find(hydro.simulation_parameters.w_extended == omega, 1, 'first');
+
 % Define excitation force based on wave conditions
-ampSpect = zeros(length(hydro.simulation_parameters.w),1);
-[~,closestIndOmega] = min(abs(omega-hydro.simulation_parameters.w));
-ampSpect(closestIndOmega) = A;
-FeRao = squeeze(hydro.hydro_coeffs.excitation.re(3,:))'*simu.rho*simu.gravity + squeeze(hydro.hydro_coeffs.excitation.im(3,:))'*simu.rho*simu.gravity*1j;
-Fexc = ampSpect.*FeRao;
+ampSpect = zeros(length(hydro.simulation_parameters.w_extended),1);
+ampSpect(omegaIndex) = A;
+Fe_re = squeeze(hydro.hydro_coeffs.excitation.re(dof, 1, :)) * simu.rho * simu.gravity;
+Fe_im = squeeze(hydro.hydro_coeffs.excitation.im(dof, 1, :)) * simu.rho * simu.gravity;
+
+Fe_interp = interp1(hydro.simulation_parameters.w, Fe_re + 1j * Fe_im, hydro.simulation_parameters.w_extended, 'spline', 'extrap')';
+Fexc = ampSpect.*Fe_interp;
 
 % Define the intrinsic mechanical impedance for the device
 mass = simu.rho*hydro.properties.volume;
-addedMass = squeeze(hydro.hydro_coeffs.added_mass.all(3,3,:))*simu.rho;
-radiationDamping = squeeze(hydro.hydro_coeffs.radiation_damping.all(3,3,:)).*squeeze(hydro.simulation_parameters.w')*simu.rho;
-hydrostaticStiffness = hydro.hydro_coeffs.linear_restoring_stiffness(3,3)*simu.rho*simu.gravity;
-Gi = -((hydro.simulation_parameters.w)'.^2.*(mass+addedMass)) + 1j*hydro.simulation_parameters.w'.*radiationDamping + hydrostaticStiffness;
-Zi = Gi./(1j*hydro.simulation_parameters.w');
+addedMass = squeeze(hydro.hydro_coeffs.added_mass.all(dof, dof, :)) * simu.rho;
+addedMass = interp1(hydro.simulation_parameters.w, addedMass, hydro.simulation_parameters.w_extended, 'spline', 'extrap')';
+
+radiationDamping = squeeze(hydro.hydro_coeffs.radiation_damping.all(dof,dof,:)).*squeeze(hydro.simulation_parameters.w')*simu.rho;
+radiationDamping = interp1(hydro.simulation_parameters.w, radiationDamping, hydro.simulation_parameters.w_extended, 'spline', 'extrap')';
+
+hydrostaticStiffness = hydro.hydro_coeffs.linear_restoring_stiffness(dof, dof) * simu.rho * simu.gravity;
+Gi = -((hydro.simulation_parameters.w_extended)'.^2 .* (mass+addedMass)) + 1j * hydro.simulation_parameters.w_extended'.*radiationDamping + hydrostaticStiffness;
+Zi = Gi./(1j*hydro.simulation_parameters.w_extended');
 
 % Calculate magnitude and phase for bode plot
 Mag = 20*log10(abs(Zi));
 Phase = (angle(Zi))*(180/pi);
 
-% Determine natural frequency based on the phase of the impedance
-[~,closestIndNat] = min(abs(imag(Zi)));
-natFreq = hydro.simulation_parameters.w(closestIndNat);
-natT = (2*pi)/natFreq;
+% Determine resonant frequency based on the phase of the impedance
+resonantFreq = interp1(Phase, hydro.simulation_parameters.w_extended, 0, 'spline','extrap');
+resonantPeriod = (2*pi)/resonantFreq;
 
 % Create bode plot for impedance
 figure()
 subplot(2,1,1)
-semilogx((hydro.simulation_parameters.w)/(2*pi),Mag)
+semilogx((hydro.simulation_parameters.w_extended)/(2*pi),Mag)
 xlabel('freq (hz)')
 ylabel('mag (dB)')
 grid on
-xline(natFreq/(2*pi))
+xline(resonantFreq/(2*pi))
 xline(1/T,'--')
-legend('','Natural Frequency','Wave Frequency','Location','southwest')
+legend('','Resonant Frequency','Wave Frequency','Location','southwest')
 
 subplot(2,1,2)
-semilogx((hydro.simulation_parameters.w)/(2*pi),Phase)
+semilogx((hydro.simulation_parameters.w_extended)/(2*pi),Phase)
 xlabel('freq (hz)')
 ylabel('phase (deg)')
 grid on
-xline(natFreq/(2*pi))
+xline(resonantFreq/(2*pi))
 xline(1/T,'--')
-legend('','Natural Frequency','Wave Frequency','Location','northwest')
+legend('','Resonant Frequency','Wave Frequency','Location','northwest')
 
 % Determine optimal latching time
-optDeclutchTime = 0.5*(natT - T)
-KpOpt = sqrt(radiationDamping(closestIndOmega)^2 + ((hydrostaticStiffness/omega) - omega*(mass + addedMass(closestIndOmega)))^2)
+optDeclutchTime = 0.5*(resonantPeriod - T)
+KpOpt = sqrt(radiationDamping(omegaIndex)^2 + ((hydrostaticStiffness/omega) - omega*(mass + addedMass(omegaIndex)))^2)
 
 % Calculate the maximum potential power
 P_max = -sum(abs(Fexc).^2./(8*real(Zi)))
diff --git a/Controls/Latching/optimalTimeCalc.m b/Controls/Latching/optimalTimeCalc.m
@@ -1,12 +1,12 @@
 % This script identifies the dynamics of the float in the respective wave 
-% conditions and determines the optimal proportional gain value for a 
-% passive controller (for regular waves)
-
+% conditions and determines the optimal proportional gain and latching time
+% value for a regular wave
 close all; clear all; clc;
 
+dof = 3;            % Caluclate for heave motion
 % Inputs (from wecSimInputFile)
 simu = simulationClass();
-body(1) = bodyClass('../hydroData/sphere.h5');
+body(1) = bodyClass('../../_Common_Input_Files/Sphere/hydroData/sphere.h5');
 waves.height = 2.5;
 waves.period = 9.6664;
 
@@ -19,56 +19,64 @@
 T = waves.period;
 omega = (1/T)*(2*pi);
 
+% Extend the freq vector to include the wave frequency
+hydro.simulation_parameters.w_extended = sort([hydro.simulation_parameters.w omega]);
+omegaIndex = find(hydro.simulation_parameters.w_extended == omega, 1, 'first');
+
 % Define excitation force based on wave conditions
-ampSpect = zeros(length(hydro.simulation_parameters.w),1);
-[~,closestIndOmega] = min(abs(omega-hydro.simulation_parameters.w));
-ampSpect(closestIndOmega) = A;
-FeRao = squeeze(hydro.hydro_coeffs.excitation.re(3,:))'*simu.rho*simu.gravity + squeeze(hydro.hydro_coeffs.excitation.im(3,:))'*simu.rho*simu.gravity*1j;
-Fexc = ampSpect.*FeRao;
+ampSpect = zeros(length(hydro.simulation_parameters.w_extended),1);
+[~,closestIndOmega] = min(abs(omega-hydro.simulation_parameters.w_extended));
+ampSpect(omegaIndex) = A;
+Fe_re = squeeze(hydro.hydro_coeffs.excitation.re(dof, 1, :)) * simu.rho * simu.gravity;
+Fe_im = squeeze(hydro.hydro_coeffs.excitation.im(dof, 1, :)) * simu.rho * simu.gravity;
+
+Fe_interp = interp1(hydro.simulation_parameters.w, Fe_re + 1j * Fe_im, hydro.simulation_parameters.w_extended, 'spline', 'extrap')';
+Fexc = ampSpect.*Fe_interp;
 
 % Define the intrinsic mechanical impedance for the device
 mass = simu.rho*hydro.properties.volume;
-addedMass = squeeze(hydro.hydro_coeffs.added_mass.all(3,3,:))*simu.rho;
-radiationDamping = squeeze(hydro.hydro_coeffs.radiation_damping.all(3,3,:)).*squeeze(hydro.simulation_parameters.w')*simu.rho;
-hydrostaticStiffness = hydro.hydro_coeffs.linear_restoring_stiffness(3,3)*simu.rho*simu.gravity;
-Gi = -((hydro.simulation_parameters.w)'.^2.*(mass+addedMass)) + 1j*hydro.simulation_parameters.w'.*radiationDamping + hydrostaticStiffness;
-Zi = Gi./(1j*hydro.simulation_parameters.w');
+addedMass = squeeze(hydro.hydro_coeffs.added_mass.all(dof, dof, :)) * simu.rho;
+addedMass = interp1(hydro.simulation_parameters.w, addedMass, hydro.simulation_parameters.w_extended, 'spline', 'extrap')';
+
+radiationDamping = squeeze(hydro.hydro_coeffs.radiation_damping.all(dof,dof,:)).*squeeze(hydro.simulation_parameters.w')*simu.rho;
+radiationDamping = interp1(hydro.simulation_parameters.w, radiationDamping, hydro.simulation_parameters.w_extended, 'spline', 'extrap')';
+
+hydrostaticStiffness = hydro.hydro_coeffs.linear_restoring_stiffness(dof, dof) * simu.rho * simu.gravity;
+Gi = -((hydro.simulation_parameters.w_extended)'.^2 .* (mass+addedMass)) + 1j * hydro.simulation_parameters.w_extended'.*radiationDamping + hydrostaticStiffness;
+Zi = Gi./(1j*hydro.simulation_parameters.w_extended');
 
 % Calculate magnitude and phase for bode plot
 Mag = 20*log10(abs(Zi));
 Phase = (angle(Zi))*(180/pi);
 
-% Determine natural frequency based on the phase of the impedance
-[~,closestIndNat] = min(abs(imag(Zi)));
-natFreq = hydro.simulation_parameters.w(closestIndNat);
-natT = (2*pi)/natFreq;
+% Determine resonant frequency based on the phase of the impedance
+resonantFreq = interp1(Phase, hydro.simulation_parameters.w_extended, 0, 'spline','extrap');
+resonantPeriod = (2*pi)/resonantFreq;
 
 % Create bode plot for impedance
 figure()
 subplot(2,1,1)
-semilogx((hydro.simulation_parameters.w)/(2*pi),Mag)
+semilogx((hydro.simulation_parameters.w_extended)/(2*pi),Mag)
 xlabel('freq (hz)')
 ylabel('mag (dB)')
 grid on
-xline(natFreq/(2*pi))
+xline(resonantFreq/(2*pi))
 xline(1/T,'--')
-legend('','Natural Frequency','Wave Frequency','Location','southwest')
+legend('','Resonant Frequency','Wave Frequency','Location','southwest')
 
 subplot(2,1,2)
-semilogx((hydro.simulation_parameters.w)/(2*pi),Phase)
+semilogx((hydro.simulation_parameters.w_extended)/(2*pi),Phase)
 xlabel('freq (hz)')
 ylabel('phase (deg)')
 grid on
-xline(natFreq/(2*pi))
+xline(resonantFreq/(2*pi))
 xline(1/T,'--')
-legend('','Natural Frequency','Wave Frequency','Location','northwest')
+legend('','Resonant Frequency','Wave Frequency','Location','northwest')
 
 % Determine optimal latching time
-optLatchTime = 0.5*(T - natT)
-KpOpt = radiationDamping(closestIndOmega)
-force = 80*(mass+addedMass(closestIndOmega))
+optLatchTime = 0.5*(T - resonantPeriod)
+KpOpt = radiationDamping(omegaIndex)
+force = 80*(mass+addedMass(omegaIndex))
 
 % Calculate the maximum potential power
-P_max = -sum(abs(Fexc).^2./(8*real(Zi)))
-
-
+P_max = -sum(abs(Fexc).^2./(8*real(Zi)))
diff --git a/Controls/Passive (P)/optimalGainCalc.m b/Controls/Passive (P)/optimalGainCalc.m
@@ -1,12 +1,12 @@
 % This script identifies the dynamics of the float in the respective wave 
 % conditions and determines the optimal proportional gain value for a 
 % passive controller (for regular waves)
-
 close all; clear all; clc;
 
+dof = 3;            % Caluclate for heave motion
 % Inputs (from wecSimInputFile)
 simu = simulationClass();
-body(1) = bodyClass('../hydroData/sphere.h5');
+body(1) = bodyClass('../../_Common_Input_Files/Sphere/hydroData/sphere.h5');
 waves.height = 2.5;
 waves.period = 9.6664; % One of periods from BEM
 
@@ -19,57 +19,70 @@
 T = waves.period;
 omega = (1/T)*(2*pi);
 
+% Extend the freq vector to include the wave frequency
+hydro.simulation_parameters.w_extended = sort([hydro.simulation_parameters.w omega]);
+omegaIndex = find(hydro.simulation_parameters.w_extended == omega, 1, 'first');
+
 % Define excitation force based on wave conditions
-ampSpect = zeros(length(hydro.simulation_parameters.w),1);
-[~,closestIndOmega] = min(abs(omega-hydro.simulation_parameters.w));
-ampSpect(closestIndOmega) = A;
-FeRao = squeeze(hydro.hydro_coeffs.excitation.re(3,:))'*simu.rho*simu.gravity + squeeze(hydro.hydro_coeffs.excitation.im(3,:))'*simu.rho*simu.gravity*1j;
-Fexc = ampSpect.*FeRao;
+ampSpect = zeros(length(hydro.simulation_parameters.w_extended),1);
+ampSpect(omegaIndex) = A;
+Fe_re = squeeze(hydro.hydro_coeffs.excitation.re(dof, 1, :)) * simu.rho * simu.gravity;
+Fe_im = squeeze(hydro.hydro_coeffs.excitation.im(dof, 1, :)) * simu.rho * simu.gravity;
+
+Fe_interp = interp1(hydro.simulation_parameters.w, Fe_re + 1j * Fe_im, hydro.simulation_parameters.w_extended, 'spline', 'extrap')';
+Fexc = ampSpect.*Fe_interp;
 
 % Define the intrinsic mechanical impedance for the device
-mass = simu.rho*hydro.properties.volume;
-addedMass = squeeze(hydro.hydro_coeffs.added_mass.all(3,3,:))*simu.rho;
-radiationDamping = squeeze(hydro.hydro_coeffs.radiation_damping.all(3,3,:)).*squeeze(hydro.simulation_parameters.w')*simu.rho;
-hydrostaticStiffness = hydro.hydro_coeffs.linear_restoring_stiffness(3,3)*simu.rho*simu.gravity;
-Gi = -((hydro.simulation_parameters.w)'.^2.*(mass+addedMass)) + 1j*hydro.simulation_parameters.w'.*radiationDamping + hydrostaticStiffness;
-Zi = Gi./(1j*hydro.simulation_parameters.w');
+mass = simu.rho * hydro.properties.volume;
+addedMass = squeeze(hydro.hydro_coeffs.added_mass.all(dof, dof, :)) * simu.rho;
+addedMass = interp1(hydro.simulation_parameters.w, addedMass, hydro.simulation_parameters.w_extended, 'spline', 'extrap')';
+addedMass = squeeze(hydro.hydro_coeffs.added_mass.inf_freq(dof, dof, :)) * simu.rho;
+
+radiationDamping = squeeze(hydro.hydro_coeffs.radiation_damping.all(dof,dof,:)).*squeeze(hydro.simulation_parameters.w')*simu.rho;
+radiationDamping = interp1(hydro.simulation_parameters.w, radiationDamping, hydro.simulation_parameters.w_extended, 'spline', 'extrap')';
+
+hydrostaticStiffness = hydro.hydro_coeffs.linear_restoring_stiffness(dof, dof) * simu.rho * simu.gravity;
+Gi = -((hydro.simulation_parameters.w_extended)'.^2 .* (mass+addedMass)) + 1j * hydro.simulation_parameters.w_extended'.*radiationDamping + hydrostaticStiffness;
+Zi = Gi./(1j*hydro.simulation_parameters.w_extended');
 
 % Calculate magnitude and phase for bode plot
 Mag = 20*log10(abs(Zi));
 Phase = (angle(Zi))*(180/pi);
 
-% Determine natural frequency based on the phase of the impedance
-[~,closestIndNat] = min(abs(imag(Zi)));
-natFreq = hydro.simulation_parameters.w(closestIndNat);
-T0 = (2*pi)/natFreq;
+% Determine resonant frequency based on the phase of the impedance
+resonantFreq = interp1(Phase, hydro.simulation_parameters.w_extended, 0, 'spline','extrap');
+resonantPeriod = (2*pi)/resonantFreq;
 
 % Create bode plot for impedance
 figure()
 subplot(2,1,1)
-semilogx((hydro.simulation_parameters.w)/(2*pi),Mag)
-xlabel('freq (hz)')
-ylabel('mag (dB)')
+semilogx((hydro.simulation_parameters.w_extended)/(2*pi),Mag)
+xlabel('freq (hz)','interpreter','latex')
+ylabel('mag (dB)','interpreter','latex')
 grid on
-xline(natFreq/(2*pi))
+xline(resonantFreq/(2*pi))
 xline(1/T,'--')
-legend('','Natural Frequency','Wave Frequency','Location','southwest')
+legend('','Resonant Frequency','Wave Frequency','Location','southwest','interpreter','latex')
 
 subplot(2,1,2)
-semilogx((hydro.simulation_parameters.w)/(2*pi),Phase)
-xlabel('freq (hz)')
-ylabel('phase (deg)')
+semilogx((hydro.simulation_parameters.w_extended)/(2*pi),Phase)
+xlabel('freq (hz)','interpreter','latex')
+ylabel('phase (deg)','interpreter','latex')
 grid on
-xline(natFreq/(2*pi))
+xline(resonantFreq/(2*pi))
 xline(1/T,'--')
-legend('','Natural Frequency','Wave Frequency','Location','northwest')
+legend('','Resonant Frequency','Wave Frequency','Location','northwest','interpreter','latex')
 
 % Calculate the maximum potential power
-P_max = -sum(abs(Fexc).^2./(8*real(Zi)))
+P_max = -sum(abs(Fexc).^2./(8*real(Zi)));
+fprintf('Maximum potential power P_max = %f\n', P_max);
 
 % Optimal proportional gain for passive control:
-KpOpt = sqrt(radiationDamping(closestIndOmega)^2 + ((hydrostaticStiffness/omega) - omega*(mass + addedMass(closestIndOmega)))^2)
+KpOpt = sqrt(radiationDamping(omegaIndex)^2 + ((hydrostaticStiffness/omega) - omega*(mass + addedMass))^2);
 Ki = 0;
+fprintf('Optimal proportional gain for passive control KpOpt = %f\n', KpOpt);
 
 % Calculate expected power with optimal passive control
 Zpto = KpOpt + Ki/(1j*omega);
-P = -sum(0.5*real(Zpto).*((abs(Fexc)).^2./(abs(Zpto+Zi)).^2))
+P = -sum(0.5*real(Zpto).*((abs(Fexc)).^2./(abs(Zpto+Zi)).^2));
+fprintf('Expected power with optimal passive control P = %f\n', P);
diff --git a/Controls/Passive (P)/userDefinedFunctionsMCR.m b/Controls/Passive (P)/userDefinedFunctionsMCR.m
@@ -17,7 +17,7 @@
 mcr.maxPower(imcr) = max(controllersOutput.power(startInd:endInd,3));
 mcr.maxForce(imcr) = max(controllersOutput.force(startInd:endInd,3));
 
-if imcr == 9
+if imcr == length(mcr.cases)
     figure()
     plot(mcr.cases,mcr.meanPower)
     title('Mean Power vs. Proportional Gain')
diff --git a/Controls/Reactive (PI)/optimalGainCalc.m b/Controls/Reactive (PI)/optimalGainCalc.m
diff --git a/Controls/Reactive (PI)/userDefinedFunctionsMCR.m b/Controls/Reactive (PI)/userDefinedFunctionsMCR.m
