diff --git a/Examples/01_turbine_model.py b/Examples/01_turbine_model.py index 27bbf7ca..5ad36661 100644 --- a/Examples/01_turbine_model.py +++ b/Examples/01_turbine_model.py @@ -1,13 +1,20 @@ """ 01_turbine_model ---------------- -Load and save a turbine model. +Interact with a ROSCO turbine model: * Read .yaml input file * Load an openfast turbine model -* Read text file with rotor performance properties +* Read text file with rotor performance properties (Cp, Ct, and Cq surface) * Print some basic turbine properties -* Save the turbine as a picklle +* Save the turbine as a pickle +* Plot the Cp surface + +.. figure:: /source/figures/01_NREL5MW_Cp.png + :align: center + :width: 70% + + Note: Uses the NREL 5MW included in the Test Cases and is a part of the OpenFAST distribution """ diff --git a/Examples/02_ccblade.py b/Examples/02_ccblade.py index 366d973d..96d7cdf0 100644 --- a/Examples/02_ccblade.py +++ b/Examples/02_ccblade.py @@ -7,7 +7,7 @@ * Read .yaml input file * Load an openfast turbine model * Run ccblade to get rotor performance properties -* Write a text file with rotor performance properties +* Write a text file (``'02_Cp_Ct_Cq.Ex03.txt'``) with rotor performance properties """ # Python modules diff --git a/Examples/03_tune_controller.py b/Examples/03_tune_controller.py index e4f0e1b8..4f9d03b0 100644 --- a/Examples/03_tune_controller.py +++ b/Examples/03_tune_controller.py @@ -5,10 +5,15 @@ In this example: * Read a .yaml file -* Load a turbine model from OpenFAST -* Tune a controller -* Write a controller input file -* Plot gain schedule +* Create a ROSCO turbine object fromt he OpenFAST model +* Tune a ROSCO controller object +* Write a controller input file (``'03_DISCON.IN'``) +* Plot gain schedule (``PC_GS_KP`` and ``PC_GS_KI`` versus ``PS_GS_angles``): + +.. figure:: /source/figures/03_GainSched.png + :align: center + :width: 70% + """ # Python modules @@ -46,7 +51,8 @@ def main(): controller.tune_controller(turbine) # Write parameter input file - param_file = os.path.join(this_dir,'DISCON.IN') + example_out_dir = os.path.join(this_dir,'examples_out') + param_file = os.path.join(example_out_dir,'03_DISCON.IN') write_DISCON(turbine,controller, param_file=param_file, txt_filename=cp_filename @@ -71,7 +77,6 @@ def main(): plt.suptitle('Pitch Controller Gains') - example_out_dir = os.path.join(this_dir,'examples_out') if not os.path.isdir(example_out_dir): os.makedirs(example_out_dir) diff --git a/Examples/04_simple_sim.py b/Examples/04_simple_sim.py index acd0848e..5c022733 100644 --- a/Examples/04_simple_sim.py +++ b/Examples/04_simple_sim.py @@ -1,17 +1,24 @@ """ 04_simple_sim ------------- -Run and plot a simple simple step wind simulation +Demonstrate the simple 1-DOF wind turbine simulator with ROSCO + In this example: -* Load turbine from saved pickle -* Tune a controller -* Run and plot a simple step wind simulation +* Load turbine from saved pickle and tune a ROSCO controller +* Run and plot a step wind simulation using 1-DOF model in ``rosco.toolbox.sim`` and the ROSCO dynamic library + +.. figure:: /source/figures/04_NREL5MW_SimpSim.png + :align: center + :width: 70% + Notes: -* You will need to have a compiled controller in ROSCO, and properly point to it in the `lib_name` variable. -* Using wind speed estimators in this simple simulation is known to cause problems. We suggesting using WE_Mode = 0 in DISCON.IN or increasing sampling rate of simulation +* You must have a compiled controller in ROSCO/rosco/lib/, and properly point to it using the `lib_name` variable. +* Using wind speed estimators in this simple simulation is known to cause problems. We suggest using WE_Mode = 0 in the DISCON.IN or increasing sampling rate of simulation as workarounds. +* The simple simulation is run twice to check that arrays are deallocated properly. + """ # Python modules import matplotlib.pyplot as plt @@ -46,7 +53,6 @@ def main(): # # Load turbine model from saved pickle turbine = ROSCO_turbine.Turbine turbine = turbine.load(os.path.join(example_out_dir,'01_NREL5MW_saved.p')) - # controller = ROSCO_controller.Controller(controller_params) # Load turbine data from OpenFAST and rotor performance text file cp_filename = os.path.join(tune_dir,path_params['rotor_performance_filename']) @@ -101,5 +107,8 @@ def main(): if False: plt.show() else: - plt.savefig(os.path.join(example_out_dir,'04_NREL5MW_SimpSim.png')) + plt.savefig(os.path.join(example_out_dir,'04_NREL5MW_SimpSim.png'), bbox_inches='tight') + +if __name__ == "__main__": + main() diff --git a/Examples/05_openfast_sim.py b/Examples/05_openfast_sim.py index c552f6d7..040874e1 100644 --- a/Examples/05_openfast_sim.py +++ b/Examples/05_openfast_sim.py @@ -1,7 +1,7 @@ """ 05_openfast_sim --------------- -Load a turbine, tune a controller, run OpenFAST simulation +Load a turbine, tune a controller, and run an OpenFAST simulation. In this example: * Load a turbine from OpenFAST @@ -10,15 +10,11 @@ Note -* you will need to have a compiled controller in ROSCO/build/ +* You must have a compiled controller in ROSCO/rosco/lib/ """ -# Python Modules -#import yaml + import os -#import numpy as np -#import matplotlib.pyplot as plt -# ROSCO toolbox modules from rosco.toolbox import controller as ROSCO_controller from rosco.toolbox import turbine as ROSCO_turbine from rosco.toolbox.utilities import write_DISCON, run_openfast @@ -52,7 +48,7 @@ def main(): controller.tune_controller(turbine) # Write parameter input file - param_file = os.path.join(this_dir,'DISCON.IN') # This must be named DISCON.IN to be seen by the compiled controller binary. + param_file = os.path.join(this_dir,'DISCON.IN') write_DISCON(turbine,controller,param_file=param_file, txt_filename=path_params['rotor_performance_filename']) # Run OpenFAST @@ -67,4 +63,4 @@ def main(): ) if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/Examples/06_peak_shaving.py b/Examples/06_peak_shaving.py index 2edfb3e5..a4f4f375 100644 --- a/Examples/06_peak_shaving.py +++ b/Examples/06_peak_shaving.py @@ -1,6 +1,6 @@ """ -06_peak_shavings ----------------- +06_peak_shaving +--------------- Load saved turbine, tune controller, plot minimum pitch schedule In this example: @@ -8,6 +8,9 @@ * Load a turbine from openfast * Tune a controller * Plot minimum pitch schedule + +.. image:: ../images/06_MinPitch.png + """ # Python modules diff --git a/Examples/14_open_loop_control.py b/Examples/14_open_loop_control.py index 2b895940..91c98378 100644 --- a/Examples/14_open_loop_control.py +++ b/Examples/14_open_loop_control.py @@ -46,7 +46,7 @@ def main(): # Set up open loop input olc = ROSCO_controller.OpenLoopControl(t_max=20) - olc.interp_timeseries( + olc.interp_series( 'blade_pitch', [0,20], [0,0.0873] , @@ -59,7 +59,7 @@ def main(): olc.sine_timeseries('nacelle_yaw', 0.0524, 60) # Plot open loop timeseries - fig,ax = olc.plot_timeseries() + fig,ax = olc.plot_series() if False: plt.show() else: @@ -144,9 +144,9 @@ def main(): valid_ind = tt > 2 # first few timesteps can differ, depending on OpenFAST solve config # Compute errors - nacelle_yaw_diff = fo['NacYaw'][valid_ind] - np.degrees(np.interp(tt[valid_ind],olc.ol_timeseries['time'],olc.ol_timeseries['nacelle_yaw'])) - bld_pitch_diff = fo['BldPitch1'][valid_ind] - np.degrees(np.interp(tt[valid_ind],olc.ol_timeseries['time'],olc.ol_timeseries['blade_pitch'])) - gen_tq_diff = fo['GenTq'][valid_ind] - np.interp(tt[valid_ind],olc.ol_timeseries['time'],olc.ol_timeseries['generator_torque'])/1e3 + nacelle_yaw_diff = fo['NacYaw'][valid_ind] - np.degrees(np.interp(tt[valid_ind],olc.ol_series['time'],olc.ol_series['nacelle_yaw'])) + bld_pitch_diff = fo['BldPitch1'][valid_ind] - np.degrees(np.interp(tt[valid_ind],olc.ol_series['time'],olc.ol_series['blade_pitch'])) + gen_tq_diff = fo['GenTq'][valid_ind] - np.interp(tt[valid_ind],olc.ol_series['time'],olc.ol_series['generator_torque'])/1e3 # Check diff timeseries np.testing.assert_allclose(nacelle_yaw_diff, 0, atol = 1e-1) # yaw has dynamics and integration error, tolerance higher diff --git a/Examples/17b_zeromq_multi_openfast.py b/Examples/17b_zeromq_multi_openfast.py index 1cd47792..6a2ee4aa 100644 --- a/Examples/17b_zeromq_multi_openfast.py +++ b/Examples/17b_zeromq_multi_openfast.py @@ -19,6 +19,7 @@ TIME_CHECK = 20 DESIRED_YAW_OFFSET = [-10, 10] +DESIRED_R_PITCH = 0.9 def main(): @@ -115,6 +116,7 @@ def __init__(self): return None def update_setpoints(self, id, current_time, measurements): + R_Pitch = 1.0 if current_time <= 10.0: YawOffset = 0.0 col_pitch_command = 0.0 @@ -122,6 +124,7 @@ def update_setpoints(self, id, current_time, measurements): col_pitch_command = np.deg2rad(2) * np.sin(0.1 * current_time) + np.deg2rad(2) # Implement dynamic induction control if id == 1: YawOffset = DESIRED_YAW_OFFSET[0] + R_Pitch = DESIRED_R_PITCH else: YawOffset = DESIRED_YAW_OFFSET[1] @@ -131,6 +134,7 @@ def update_setpoints(self, id, current_time, measurements): setpoints['ZMQ_PitOffset(1)'] = col_pitch_command setpoints['ZMQ_PitOffset(2)'] = col_pitch_command setpoints['ZMQ_PitOffset(3)'] = col_pitch_command + setpoints['ZMQ_R_Pitch'] = R_Pitch return setpoints @@ -149,6 +153,8 @@ def sim_openfast_1(): r.controller_params["DISCON"] = {} r.controller_params["DISCON"]["ZMQ_Mode"] = 1 r.controller_params["DISCON"]["ZMQ_ID"] = 1 + r.controller_params['DISCON']['PRC_Mode'] = 1 + r.controller_params['DISCON']['PRC_Comm'] = 2 r.save_dir = run_dir r.run_FAST() @@ -169,6 +175,8 @@ def sim_openfast_2(): r.controller_params["LoggingLevel"] = 2 r.controller_params["DISCON"]["ZMQ_Mode"] = 1 r.controller_params["DISCON"]["ZMQ_ID"] = 2 + r.controller_params['DISCON']['PRC_Mode'] = 1 + r.controller_params['DISCON']['PRC_Comm'] = 2 r.run_FAST() diff --git a/Examples/20_active_wake_control.py b/Examples/20_active_wake_control.py index 85fca9d8..20e51126 100644 --- a/Examples/20_active_wake_control.py +++ b/Examples/20_active_wake_control.py @@ -3,7 +3,7 @@ ---------------------- Run openfast with ROSCO and active wake control Set up and run simulation with AWC, check outputs -Active wake control (AWC) with blade pitching is implemented in this example with two approaches as detailed below: +Active wake control (AWC) with blade pitching is implemented in this example with two approaches as detailed in the python script. """ # ----------------------------------------------- # AWC_Mode = 1: Normal mode method: diff --git a/Examples/22_cable_control.py b/Examples/22_cable_control.py index e5e23885..8c994216 100644 --- a/Examples/22_cable_control.py +++ b/Examples/22_cable_control.py @@ -72,21 +72,21 @@ def main(): line_ends = [-14.51, 1.58, 10.33] olc = OpenLoopControl(t_max=t_max) - olc.interp_timeseries( + olc.interp_series( 'cable_control_1', [0,t_trans,t_trans+t_sigma], [0,0,line_ends[0]] , 'sigma' ) - olc.interp_timeseries( + olc.interp_series( 'cable_control_2', [0,t_trans,t_trans+t_sigma], [0,0,line_ends[1]] , 'sigma' ) - olc.interp_timeseries( + olc.interp_series( 'cable_control_3', [0,t_trans,t_trans+t_sigma], [0,0,line_ends[2]] , diff --git a/Examples/23_structural_control.py b/Examples/23_structural_control.py index 96569c93..0ed3632c 100644 --- a/Examples/23_structural_control.py +++ b/Examples/23_structural_control.py @@ -60,21 +60,21 @@ def main(): applied_force = [-2e6, 1e6, 1e6] olc = OpenLoopControl(t_max=t_max) - olc.interp_timeseries( + olc.interp_series( 'struct_control_1', [0,t_trans,t_trans+t_sigma], [0,0,applied_force[0]] , 'sigma' ) - olc.interp_timeseries( + olc.interp_series( 'struct_control_2', [0,t_trans,t_trans+t_sigma], [0,0,applied_force[1]] , 'sigma' ) - olc.interp_timeseries( + olc.interp_series( 'struct_control_3', [0,t_trans,t_trans+t_sigma], [0,0,applied_force[2]] , diff --git a/Examples/25_rotor_position_control.py b/Examples/25_rotor_position_control.py index d1449d59..868fd1d9 100644 --- a/Examples/25_rotor_position_control.py +++ b/Examples/25_rotor_position_control.py @@ -44,12 +44,12 @@ def main(): fast_out = op.load_fast_out(os.path.join(run_dir,'NREL2p8/power_curve/base/NREL2p8_0.outb'), tmin=0) olc = OpenLoopControl() - olc.ol_timeseries['time'] = fast_out[0]['Time'] - olc.ol_timeseries['blade_pitch1'] = np.radians(fast_out[0]['BldPitch1']) - olc.ol_timeseries['blade_pitch2'] = np.radians(fast_out[0]['BldPitch2']) - olc.ol_timeseries['blade_pitch3'] = np.radians(fast_out[0]['BldPitch3']) - olc.ol_timeseries['generator_torque'] = fast_out[0]['GenTq'] * 1000 - olc.ol_timeseries['azimuth'] = np.radians(fast_out[0]['Azimuth']) + olc.ol_series['time'] = fast_out[0]['Time'] + olc.ol_series['blade_pitch1'] = np.radians(fast_out[0]['BldPitch1']) + olc.ol_series['blade_pitch2'] = np.radians(fast_out[0]['BldPitch2']) + olc.ol_series['blade_pitch3'] = np.radians(fast_out[0]['BldPitch3']) + olc.ol_series['generator_torque'] = fast_out[0]['GenTq'] * 1000 + olc.ol_series['azimuth'] = np.radians(fast_out[0]['Azimuth']) # Save initial RotSpeed, Azimuth for later RotSpeed_0 = fast_out[0]['RotSpeed'][0] diff --git a/Examples/26_marine_hydro.py b/Examples/26_marine_hydro.py index 24abfc3f..46cb4fbb 100644 --- a/Examples/26_marine_hydro.py +++ b/Examples/26_marine_hydro.py @@ -7,11 +7,7 @@ import os from rosco.toolbox.ofTools.case_gen.run_FAST import run_FAST_ROSCO from rosco.toolbox.ofTools.case_gen import CaseLibrary as cl -#from rosco.toolbox.ofTools.fast_io import output_processing -#from rosco.toolbox.ofTools.fast_io.FAST_reader import InputReader_OpenFAST -#from rosco.toolbox.inputs.validation import load_rosco_yaml -#import matplotlib.pyplot as plt -#from rosco.toolbox.controller import OpenLoopControl + def main(): #directories @@ -40,35 +36,10 @@ def main(): # r.controller_params = controller_params r.save_dir = run_dir r.rosco_dir = rosco_dir + # r.rosco_dll = '/Users/dzalkind/Tools/ROSCO-PRC/rosco/controller/build/libdiscon.dylib' r.run_FAST() - print('here') - - - # op = output_processing.output_processing() - # op2 = output_processing.output_processing() - - # md_out = op.load_fast_out([os.path.join(run_dir,'IEA15MW_cable/power_curve/base/IEA15MW_cable_0.MD.Line1.out')], tmin=0) - # local_vars = op2.load_fast_out([os.path.join(run_dir,'IEA15MW_cable/power_curve/base/IEA15MW_cable_0.RO.dbg2')], tmin=0) - - # fig, axs = plt.subplots(4,1) - # axs[0].plot(local_vars[0]['Time'],local_vars[0]['CC_DesiredL'],label='CC_DesiredL') - # axs[1].plot(local_vars[0]['Time'],local_vars[0]['CC_ActuatedL'],label='CC_ActuatedL') - # axs[2].plot(local_vars[0]['Time'],local_vars[0]['CC_ActuatedDL'],label='CC_ActuatedDL') - # axs[3].plot(md_out[0]['Time'],md_out[0]['Seg20Lst'],label='Seg20Lst') - - # [a.legend() for a in axs] - # [a.grid() for a in axs] - - # if False: - # plt.show() - # else: - # plt.savefig(os.path.join(example_out_dir,'22_cable_control.png')) - - # Check that the last segment of line 1 shrinks by 10 m - # np.testing.assert_almost_equal(md_out[0]['Seg20Lst'][-1] - md_out[0]['Seg20Lst'][0], line_ends[0], 2) - if __name__=="__main__": diff --git a/Examples/27_power_ref_control.py b/Examples/27_soft_cut_out.py similarity index 81% rename from Examples/27_power_ref_control.py rename to Examples/27_soft_cut_out.py index d6164234..4f9b34bc 100644 --- a/Examples/27_power_ref_control.py +++ b/Examples/27_soft_cut_out.py @@ -1,9 +1,11 @@ """ -27_power_ref_control --------------------- -Run openfast with ROSCO and cable control -Demonstrate a simulation with a generator reference speed that changes with estimated wind speed -Set reference rotor speed as a function of wind speed (estimate in ROSCO) +27_soft_cut_out +--------------- +Set up a control input to do a soft cut-out of a wind turbine at high wind speeds. +This example uses the first power reference control implementation (PRC_Mode of 1), where the user specifies the speed setpoint versus wind speed. +We can use this to track specific rotor speeds based on the wind speed, or de-rate/power boost using by changing the speed. +With PRC_Mode of 2, we can do this but also change the power rating with pitch and torque. + """ import os @@ -58,7 +60,7 @@ def main(): r = run_FAST_ROSCO() r.tuning_yaml = parameter_filename - r.wind_case_fcn = cl.ramp # single step wind input + r.wind_case_fcn = cl.ramp # ramp wind input if FULL_TEST: # Full test diff --git a/Examples/29_power_control.py b/Examples/29_power_control.py new file mode 100644 index 00000000..71a8e129 --- /dev/null +++ b/Examples/29_power_control.py @@ -0,0 +1,227 @@ +''' +29_power_control +---------------- +This example demonstrates an advanced method for controlling the power output of a turbine. +Users may want to reduce the power output, or de-rate the turbine to reduce loads, increase wind speeds deeper in the farm, or for grid support functions. +Users might also want to increase the power output when it's safe to do so. +Future advanced control methods can make use of these inputs to directly account for the trade off between power and loads. + +There are a few ways to control the turbine power output + +The power rating (``R``) is the controlled power relative to the rated power (or available power below rated). +For example R = 0.9, will produce 90% of the rated power (or available power below rated). + +* Speed (``R_Speed``) will change the rated speed, or the speed setpoint relative to the optimal tip speed ratio (below rated). +* Torque (``R_Torque``): will change the rated torque. In constant power operation (`VS_ConstPower` of 1), the torque is non-constant, but the rated power used to calcualte the torque demand is adapted accordingly. +* Pitch (``R_Pitch``): will change the minimum pitch angle of the turbine. When using peak shaving, the min pitch is the maximum of the minimum pitch for peak shaving (``PS_Min_Pitch``) and the minimum pitch for power control (``PRC_Min_Pitch``). The ROSCO toolbox can generate a lookup table from ``R_Pitch`` to ``PRC_Min_Pitch`` using the Cp surface. + +The three methods are compared in the following figure: + +.. image:: ../images/29_PRC_Methods.png + +The power rating can be controlled with three different "communication" methods (``PRC_Comm``), via: + +#. Constant settings in the DISCON: ``R_Speed``, ``R_Torque``, and ``R_Pitch``. +#. Open-loop control inputs with time or wind speed breakpoints. Two applications will be shown in the example below. +#. The ZeroMQ interface. A simple example is provided in ``17b_zeromq_multi_openfast.py``. + +This example shows users how to set up open loop control inputs for + +#. A start up routine that ramps the turbine rating using R_Torque in steps over 400 seconds. +#. A soft cut-out routine for high wind speeds. +#. Active wake control for above rated operation, using a sinusoidal input for R_speed. + + +Start Up Demo +`````````````` + +.. image:: ../images/29_StartUp.png + +The turbine is started in a parked configuration with the blades pitched to 90 degrees. +R_Torque is increased from 0 to 0.2, then from 0.2 to 1.0, simulating a startup routine. +The torque, because it is saturated at the max torque, which is scaled by ``R_Torque`` follows the trajectory of ``PRC_R_Torque``. +``PRC_R_Torque`` is the variable name inside ROSCO and the ``.dbg2`` file. +The blade pitch controller is active throughout the simulation, regulating the generator to the rated speed as usual. + + +Soft Cut-out Demo +````````````````` +.. image:: ../images/29_Soft_Cutout.png + +The turbine starts in above-rated operation and a ramp wind input goes well beyond the normal cut-out wind speed of 25 m/s. +In this demonstration, we use both R_Speed and R_Torque to ramp the turbine rating from 1.0 at 20 m/s to 0.5 at 30 m/s and 0.0 at 40 m/s. +The signals ``PRC_R_Speed`` and ``PRC_R_Torque`` ramp towards 0 following the R versus wind speed table. +Both GenTq and GenSpeed drop to 0 following the reference signals and the power follows. + + +Active Wake Control Demo +````````````````````````` + +.. image:: ../images/29_AWC.png + +In active wake control, the goal is to change the rotor thrust through low frequency changes to the blade pitch. +Since the blade pitch must be used to control the rotor speed in above rated operation, we can insteady vary the generator speed reference (via R_Speed) to create similar blade pitch variations. + +''' + +import os +from rosco.toolbox.ofTools.case_gen.run_FAST import run_FAST_ROSCO +from rosco.toolbox.ofTools.case_gen import CaseLibrary as cl +from rosco.toolbox import controller as ROSCO_controller + + +import numpy as np + +rpm2RadSec = 2.0*(np.pi)/60.0 + + +#directories +this_dir = os.path.dirname(os.path.abspath(__file__)) +rosco_dir = os.path.dirname(this_dir) +example_out_dir = os.path.join(this_dir,'examples_out') +os.makedirs(example_out_dir,exist_ok=True) + + +def main(): + + FULL_TEST = False # FULL_TEST for local testing, otherwise shorter for CI + + # Input yaml and output directory + parameter_filename = os.path.join(this_dir,'Tune_Cases/IEA15MW.yaml') + + + # Set DISCON input dynamically through yaml/dict + controller_params = {} + controller_params['DISCON'] = {} + controller_params['DISCON']['Echo'] = 1 + controller_params['DISCON']['PRC_Mode'] = 2 + controller_params['DISCON']['PRC_Comm'] = 1 + controller_params['DISCON']['PRC_R_Torque'] = 1.0 + controller_params['DISCON']['PRC_R_Speed'] = 1.0 + controller_params['DISCON']['PRC_R_Pitch'] = 1.0 + controller_params['DISCON']['OL_BP_FiltFreq'] = 2 * np.pi / 30 + + # simulation set up + r = run_FAST_ROSCO() + r.tuning_yaml = parameter_filename + r.case_inputs = {} + + # Disable floating DOFs for clarity + r.case_inputs[('ElastoDyn','PtfmSgDOF')] = {'vals': ['False'], 'group': 0} + r.case_inputs[('ElastoDyn','PtfmSwDOF')] = {'vals': ['False'], 'group': 0} + r.case_inputs[('ElastoDyn','PtfmHvDOF')] = {'vals': ['False'], 'group': 0} + r.case_inputs[('ElastoDyn','PtfmRDOF')] = {'vals': ['False'], 'group': 0} + r.case_inputs[('ElastoDyn','PtfmPDOF')] = {'vals': ['False'], 'group': 0} + r.case_inputs[('ElastoDyn','PtfmYDOF')] = {'vals': ['False'], 'group': 0} + + + sim_config = 1 + + # 1. Soft start up + if sim_config == 1: + if FULL_TEST: + t_max = 500 + else: # Shorter for ROSCO CI + t_max = 100 + + run_dir = os.path.join(example_out_dir,'29_PRC_Demo/1_Soft_Start') + olc = ROSCO_controller.OpenLoopControl(t_max=t_max) + + if FULL_TEST: + olc.interp_series( + 'R_torque', + [0,100,200,300,400], + [0,0.0,0.2,0.2,1] , + 'sigma' + ) + else: + olc.interp_series( + 'R_torque', + [0,100], + [0,1.0] , + 'sigma' + ) + + # Wind case + r.wind_case_fcn = cl.power_curve + r.wind_case_opts = { + 'U': [14], # from 10 to 15 m/s + 'TMax': t_max, + } + r.case_inputs[('ElastoDyn','BlPitch1')] = {'vals': [90.], 'group': 0} + r.case_inputs[('ElastoDyn','BlPitch2')] = {'vals': [90.], 'group': 0} + r.case_inputs[('ElastoDyn','BlPitch3')] = {'vals': [90.], 'group': 0} + r.case_inputs[('ElastoDyn','RotSpeed')] = {'vals': [0.], 'group': 0} + + [("ElastoDyn","BlPitch1")] + + # 2. Soft cut out + if sim_config == 2: + run_dir = os.path.join(example_out_dir,'29_PRC_Demo/2_Use_Torque') + olc = ROSCO_controller.OpenLoopControl( + breakpoint='wind_speed', + u_min = 20, + u_max = 40 + ) + olc.interp_series( + 'R_speed', + [20,30,40], + [1,0.707,0.0] , + ) + olc.interp_series( + 'R_torque', + [20,30,40], + [1,0.707,0.0] , + ) + r.wind_case_fcn = cl.ramp + r.wind_case_opts = { + 'U_start': 20, # from 10 to 15 m/s + 'U_end': 50, + 't_start': 500, + 't_end': 2500, + } + + r.case_inputs[('ElastoDyn','BlPitch1')] = {'vals': [19.], 'group': 0} + r.case_inputs[('ElastoDyn','BlPitch2')] = {'vals': [19.], 'group': 0} + r.case_inputs[('ElastoDyn','BlPitch3')] = {'vals': [19.], 'group': 0} + r.case_inputs[('ElastoDyn','RotSpeed')] = {'vals': [7.], 'group': 0} + + controller_params['DISCON']['OL_BP_FiltFreq'] = 1/100 + controller_params['DISCON']['VS_MinOMSpd'] = 0 # required to force low reference speed + + # AWC above rated via R_Speed/Torque + if sim_config == 3: + run_dir = os.path.join(example_out_dir,'29_PRC_Demo/3_AWC_Above_Rated') + t_max = 600 + r.wind_case_fcn = cl.power_curve + r.wind_case_opts = { + 'U': [14], # from 10 to 15 m/s + 'TMax': t_max, + } + olc = ROSCO_controller.OpenLoopControl(t_max=600) + olc.sine_timeseries( + 'R_speed', # could use R_speed or R_torque + 0.05, # amplitude, fraction of rated power + 100, # period, sec + 1, # offset, (-) + ) + + + fig,ax = olc.plot_series() + fig.savefig(os.path.join(example_out_dir,'29_OL_Inputs.png')) + + # Write open loop input, get OL indices + os.makedirs(run_dir,exist_ok=True) + ol_filename = os.path.join(run_dir,'29_OL_Input.dat') + ol_dict = olc.write_input(ol_filename) + controller_params['open_loop'] = ol_dict + controller_params['OL_Mode'] = 1 + + r.controller_params = controller_params + r.save_dir = run_dir + r.run_FAST() + + + +if __name__=="__main__": + main() diff --git a/Examples/Test_Cases/BAR_10/BAR_10_DISCON.IN b/Examples/Test_Cases/BAR_10/BAR_10_DISCON.IN index 49d1d8be..d79e7b4e 100644 --- a/Examples/Test_Cases/BAR_10/BAR_10_DISCON.IN +++ b/Examples/Test_Cases/BAR_10/BAR_10_DISCON.IN @@ -1,5 +1,5 @@ ! Controller parameter input file for the BAR_10 wind turbine -! - File written using ROSCO version 2.9.4 controller tuning logic on 07/01/24 +! - File written using ROSCO version 2.9.4 controller tuning logic on 09/03/24 !------- SIMULATION CONTROL ------------------------------------------------------------ 1 ! LoggingLevel - {0: write no debug files, 1: write standard output .dbg-file, 2: LoggingLevel 1 + ROSCO LocalVars (.dbg2) 3: LoggingLevel 2 + complete avrSWAP-array (.dbg3)} @@ -15,7 +15,7 @@ 1 ! PC_ControlMode - Blade pitch control mode {0: No pitch, fix to fine pitch, 1: active PI blade pitch control} 0 ! Y_ControlMode - Yaw control mode {0: no yaw control, 1: yaw rate control, 2: yaw-by-IPC} 1 ! SS_Mode - Setpoint Smoother mode {0: no setpoint smoothing, 1: introduce setpoint smoothing} -0 ! PRC_Mode - Power reference tracking mode{0: use standard rotor speed set points, 1: use PRC rotor speed setpoints} +0 ! PRC_Mode - Power reference tracking mode{0: power control disabled, 1: lookup table from wind speed to generator speed setpoints, 2: change speed, torque, pitch to control power} 2 ! WE_Mode - Wind speed estimator mode {0: One-second low pass filtered hub height wind speed, 1: Immersion and Invariance Estimator, 2: Extended Kalman Filter} 1 ! PS_Mode - Pitch saturation mode {0: no pitch saturation, 1: implement pitch saturation} 0 ! SD_Mode - Shutdown mode {0: no shutdown procedure, 1: pitch to max pitch at shutdown} @@ -77,7 +77,7 @@ !------- VS TORQUE CONTROL ------------------------------------------------ 98.00000000000 ! VS_GenEff - Generator efficiency mechanical power -> electrical power, [should match the efficiency defined in the generator properties!], [%] 63892.81326000 ! VS_ArSatTq - Above rated generator torque PI control saturation, [Nm] -4500000.000000 ! VS_MaxRat - Maximum torque rate (in absolute value) in torque controller, [Nm/s]. +62000.00000000 ! VS_MaxRat - Maximum torque rate (in absolute value) in torque controller, [Nm/s]. 70282.09458000 ! VS_MaxTq - Maximum generator torque in Region 3 (HSS side), [Nm]. 0.000000000000 ! VS_MinTq - Minimum generator torque (HSS side), [Nm]. 27.49717000000 ! VS_MinOMSpd - Minimum generator speed [rad/s] @@ -96,6 +96,13 @@ 0.00100 ! SS_PCGain - Collective pitch controller setpoint smoother gain, [-]. !------- POWER REFERENCE TRACKING -------------------------------------- +0 ! PRC_Comm - Power reference communication mode when PRC_Mode = 2, 0- use constant DISCON inputs, 1- use open loop inputs, 2- use ZMQ inputs +1.00000 ! PRC_R_Torque - Constant power rating through changing the rated torque, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Speed - Constant power rating through changing the rated generator speed, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Pitch - Constant power rating through changing the fine pitch angle, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective below rated [-] +20 ! PRC_Table_n - Number of elements in PRC_R to _Pitch table. Used if PRC_Mode = 1. +0.0000 0.0526 0.1053 0.1579 0.2105 0.2632 0.3158 0.3684 0.4211 0.4737 0.5263 0.5789 0.6316 0.6842 0.7368 0.7895 0.8421 0.8947 0.9474 1.0000 ! PRC_R_Table - Table of turbine rating versus fine pitch (PRC_Pitch_Table), length should be PRC_Table_n, default is 1 [-]. Used if PRC_Mode = 1. +0.1766 0.1707 0.1647 0.1586 0.1522 0.1458 0.1391 0.1323 0.1252 0.1178 0.1102 0.1022 0.0939 0.0850 0.0755 0.0655 0.0538 0.0405 0.0238 -0.0123 ! PRC_Pitch_Table - Table of fine pitch versus PRC_R_Table, length should be PRC_Table_n [rad]. Used if PRC_Mode = 1. 2 ! PRC_n - Number of elements in PRC_WindSpeeds and PRC_GenSpeeds array 0.07854 ! PRC_LPF_Freq - Frequency of the low pass filter on the wind speed estimate used to set PRC_GenSpeeds [rad/s] 3.0000 25.0000 ! PRC_WindSpeeds - Array of wind speeds used in rotor speed vs. wind speed lookup table [m/s] @@ -154,6 +161,8 @@ !------- Open Loop Control ----------------------------------------------------- "unused" ! OL_Filename - Input file with open loop timeseries (absolute path or relative to this file) +0 ! OL_BP_Mode - Breakpoint mode for open loop control, 0 - indexed by time (default), 1 - indexed by wind speed] +0.000000 ! OL_BP_FiltFreq - Natural frequency of 1st order filter on breakpoint for open loop control. 0 will skip filter. 0 ! Ind_Breakpoint - The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1) 0 0 0 ! Ind_BldPitch - The columns in OL_Filename that contains the blade pitch (1,2,3) inputs in rad [array] 0 ! Ind_GenTq - The column in OL_Filename that contains the generator torque in Nm @@ -162,6 +171,9 @@ 0.0000 0.0000 0.0000 0.0000 ! RP_Gains - PID gains and Tf of derivative for rotor position control (used if OL_Mode = 2) 0 ! Ind_CableControl - The column(s) in OL_Filename that contains the cable control inputs in m [Used with CC_Mode = 2, must be the same size as CC_Group_N] 0 ! Ind_StructControl - The column(s) in OL_Filename that contains the structural control inputs [Used with StC_Mode = 2, must be the same size as StC_Group_N] +0 ! Ind_R_Speed - Index (column, 1-indexed) of power rating via speed offset +0 ! Ind_R_Torque - Index (column, 1-indexed) of power rating via torque offset +0 ! Ind_R_Pitch - Index (column, 1-indexed) of power rating via pitch offset !------- Pitch Actuator Model ----------------------------------------------------- 3.140000000000 ! PA_CornerFreq - Pitch actuator bandwidth/cut-off frequency [rad/s] diff --git a/Examples/Test_Cases/IEA-15-240-RWT-UMaineSemi/DISCON-UMaineSemi.IN b/Examples/Test_Cases/IEA-15-240-RWT-UMaineSemi/DISCON-UMaineSemi.IN index a55a49d0..ed348ab9 100644 --- a/Examples/Test_Cases/IEA-15-240-RWT-UMaineSemi/DISCON-UMaineSemi.IN +++ b/Examples/Test_Cases/IEA-15-240-RWT-UMaineSemi/DISCON-UMaineSemi.IN @@ -1,5 +1,5 @@ ! Controller parameter input file for the IEA-15-240-RWT-UMaineSemi wind turbine -! - File written using ROSCO version 2.9.4 controller tuning logic on 07/01/24 +! - File written using ROSCO version 2.9.4 controller tuning logic on 09/03/24 !------- SIMULATION CONTROL ------------------------------------------------------------ 2 ! LoggingLevel - {0: write no debug files, 1: write standard output .dbg-file, 2: LoggingLevel 1 + ROSCO LocalVars (.dbg2) 3: LoggingLevel 2 + complete avrSWAP-array (.dbg3)} @@ -15,7 +15,7 @@ 1 ! PC_ControlMode - Blade pitch control mode {0: No pitch, fix to fine pitch, 1: active PI blade pitch control} 0 ! Y_ControlMode - Yaw control mode {0: no yaw control, 1: yaw rate control, 2: yaw-by-IPC} 1 ! SS_Mode - Setpoint Smoother mode {0: no setpoint smoothing, 1: introduce setpoint smoothing} -0 ! PRC_Mode - Power reference tracking mode{0: use standard rotor speed set points, 1: use PRC rotor speed setpoints} +0 ! PRC_Mode - Power reference tracking mode{0: power control disabled, 1: lookup table from wind speed to generator speed setpoints, 2: change speed, torque, pitch to control power} 2 ! WE_Mode - Wind speed estimator mode {0: One-second low pass filtered hub height wind speed, 1: Immersion and Invariance Estimator, 2: Extended Kalman Filter} 1 ! PS_Mode - Pitch saturation mode {0: no pitch saturation, 1: implement pitch saturation} 0 ! SD_Mode - Shutdown mode {0: no shutdown procedure, 1: pitch to max pitch at shutdown} @@ -96,6 +96,13 @@ 0.00100 ! SS_PCGain - Collective pitch controller setpoint smoother gain, [-]. !------- POWER REFERENCE TRACKING -------------------------------------- +0 ! PRC_Comm - Power reference communication mode when PRC_Mode = 2, 0- use constant DISCON inputs, 1- use open loop inputs, 2- use ZMQ inputs +1.00000 ! PRC_R_Torque - Constant power rating through changing the rated torque, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Speed - Constant power rating through changing the rated generator speed, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Pitch - Constant power rating through changing the fine pitch angle, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective below rated [-] +20 ! PRC_Table_n - Number of elements in PRC_R to _Pitch table. Used if PRC_Mode = 1. +0.0000 0.0526 0.1053 0.1579 0.2105 0.2632 0.3158 0.3684 0.4211 0.4737 0.5263 0.5789 0.6316 0.6842 0.7368 0.7895 0.8421 0.8947 0.9474 1.0000 ! PRC_R_Table - Table of turbine rating versus fine pitch (PRC_Pitch_Table), length should be PRC_Table_n, default is 1 [-]. Used if PRC_Mode = 1. +0.2296 0.2222 0.2144 0.2066 0.1984 0.1902 0.1814 0.1726 0.1633 0.1538 0.1439 0.1334 0.1226 0.1112 0.0989 0.0858 0.0715 0.0552 0.0351 0.0000 ! PRC_Pitch_Table - Table of fine pitch versus PRC_R_Table, length should be PRC_Table_n [rad]. Used if PRC_Mode = 1. 2 ! PRC_n - Number of elements in PRC_WindSpeeds and PRC_GenSpeeds array 0.07854 ! PRC_LPF_Freq - Frequency of the low pass filter on the wind speed estimate used to set PRC_GenSpeeds [rad/s] 3.0000 25.0000 ! PRC_WindSpeeds - Array of wind speeds used in rotor speed vs. wind speed lookup table [m/s] @@ -154,6 +161,8 @@ !------- Open Loop Control ----------------------------------------------------- "unused" ! OL_Filename - Input file with open loop timeseries (absolute path or relative to this file) +0 ! OL_BP_Mode - Breakpoint mode for open loop control, 0 - indexed by time (default), 1 - indexed by wind speed] +0.000000 ! OL_BP_FiltFreq - Natural frequency of 1st order filter on breakpoint for open loop control. 0 will skip filter. 0 ! Ind_Breakpoint - The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1) 0 0 0 ! Ind_BldPitch - The columns in OL_Filename that contains the blade pitch (1,2,3) inputs in rad [array] 0 ! Ind_GenTq - The column in OL_Filename that contains the generator torque in Nm @@ -162,6 +171,9 @@ 0.0000 0.0000 0.0000 0.0000 ! RP_Gains - PID gains and Tf of derivative for rotor position control (used if OL_Mode = 2) 0 ! Ind_CableControl - The column(s) in OL_Filename that contains the cable control inputs in m [Used with CC_Mode = 2, must be the same size as CC_Group_N] 0 ! Ind_StructControl - The column(s) in OL_Filename that contains the structural control inputs [Used with StC_Mode = 2, must be the same size as StC_Group_N] +0 ! Ind_R_Speed - Index (column, 1-indexed) of power rating via speed offset +0 ! Ind_R_Torque - Index (column, 1-indexed) of power rating via torque offset +0 ! Ind_R_Pitch - Index (column, 1-indexed) of power rating via pitch offset !------- Pitch Actuator Model ----------------------------------------------------- 1.570800000000 ! PA_CornerFreq - Pitch actuator bandwidth/cut-off frequency [rad/s] diff --git a/Examples/Test_Cases/MHK_RM1/MHK_RM1_DISCON.IN b/Examples/Test_Cases/MHK_RM1/MHK_RM1_DISCON.IN index e23b75b5..89395380 100644 --- a/Examples/Test_Cases/MHK_RM1/MHK_RM1_DISCON.IN +++ b/Examples/Test_Cases/MHK_RM1/MHK_RM1_DISCON.IN @@ -1,5 +1,5 @@ ! Controller parameter input file for the MHK_RM1_Floating wind turbine -! - File written using ROSCO version 2.9.4 controller tuning logic on 07/01/24 +! - File written using ROSCO version 2.9.4 controller tuning logic on 09/03/24 !------- SIMULATION CONTROL ------------------------------------------------------------ 2 ! LoggingLevel - {0: write no debug files, 1: write standard output .dbg-file, 2: LoggingLevel 1 + ROSCO LocalVars (.dbg2) 3: LoggingLevel 2 + complete avrSWAP-array (.dbg3)} @@ -15,7 +15,7 @@ 1 ! PC_ControlMode - Blade pitch control mode {0: No pitch, fix to fine pitch, 1: active PI blade pitch control} 0 ! Y_ControlMode - Yaw control mode {0: no yaw control, 1: yaw rate control, 2: yaw-by-IPC} 1 ! SS_Mode - Setpoint Smoother mode {0: no setpoint smoothing, 1: introduce setpoint smoothing} -0 ! PRC_Mode - Power reference tracking mode{0: use standard rotor speed set points, 1: use PRC rotor speed setpoints} +0 ! PRC_Mode - Power reference tracking mode{0: power control disabled, 1: lookup table from wind speed to generator speed setpoints, 2: change speed, torque, pitch to control power} 0 ! WE_Mode - Wind speed estimator mode {0: One-second low pass filtered hub height wind speed, 1: Immersion and Invariance Estimator, 2: Extended Kalman Filter} 0 ! PS_Mode - Pitch saturation mode {0: no pitch saturation, 1: implement pitch saturation} 0 ! SD_Mode - Shutdown mode {0: no shutdown procedure, 1: pitch to max pitch at shutdown} @@ -77,7 +77,7 @@ !------- VS TORQUE CONTROL ------------------------------------------------ 94.40000000000 ! VS_GenEff - Generator efficiency mechanical power -> electrical power, [should match the efficiency defined in the generator properties!], [%] 8300.335630000 ! VS_ArSatTq - Above rated generator torque PI control saturation, [Nm] -1500000.000000 ! VS_MaxRat - Maximum torque rate (in absolute value) in torque controller, [Nm/s]. +7800.000000000 ! VS_MaxRat - Maximum torque rate (in absolute value) in torque controller, [Nm/s]. 12450.50344000 ! VS_MaxTq - Maximum generator torque in Region 3 (HSS side), [Nm]. 0.000000000000 ! VS_MinTq - Minimum generator torque (HSS side), [Nm]. 19.00309000000 ! VS_MinOMSpd - Minimum generator speed [rad/s] @@ -96,6 +96,13 @@ 0.00100 ! SS_PCGain - Collective pitch controller setpoint smoother gain, [-]. !------- POWER REFERENCE TRACKING -------------------------------------- +0 ! PRC_Comm - Power reference communication mode when PRC_Mode = 2, 0- use constant DISCON inputs, 1- use open loop inputs, 2- use ZMQ inputs +1.00000 ! PRC_R_Torque - Constant power rating through changing the rated torque, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Speed - Constant power rating through changing the rated generator speed, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Pitch - Constant power rating through changing the fine pitch angle, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective below rated [-] +20 ! PRC_Table_n - Number of elements in PRC_R to _Pitch table. Used if PRC_Mode = 1. +0.0000 0.0526 0.1053 0.1579 0.2105 0.2632 0.3158 0.3684 0.4211 0.4737 0.5263 0.5789 0.6316 0.6842 0.7368 0.7895 0.8421 0.8947 0.9474 1.0000 ! PRC_R_Table - Table of turbine rating versus fine pitch (PRC_Pitch_Table), length should be PRC_Table_n, default is 1 [-]. Used if PRC_Mode = 1. +0.1702 0.1654 0.1607 0.1556 0.1505 0.1453 0.1398 0.1341 0.1284 0.1221 0.1157 0.1089 0.1016 0.0939 0.0853 0.0760 0.0656 0.0528 0.0364 0.0013 ! PRC_Pitch_Table - Table of fine pitch versus PRC_R_Table, length should be PRC_Table_n [rad]. Used if PRC_Mode = 1. 2 ! PRC_n - Number of elements in PRC_WindSpeeds and PRC_GenSpeeds array 0.07854 ! PRC_LPF_Freq - Frequency of the low pass filter on the wind speed estimate used to set PRC_GenSpeeds [rad/s] 3.0000 25.0000 ! PRC_WindSpeeds - Array of wind speeds used in rotor speed vs. wind speed lookup table [m/s] @@ -154,6 +161,8 @@ !------- Open Loop Control ----------------------------------------------------- "unused" ! OL_Filename - Input file with open loop timeseries (absolute path or relative to this file) +0 ! OL_BP_Mode - Breakpoint mode for open loop control, 0 - indexed by time (default), 1 - indexed by wind speed] +0.000000 ! OL_BP_FiltFreq - Natural frequency of 1st order filter on breakpoint for open loop control. 0 will skip filter. 0 ! Ind_Breakpoint - The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1) 0 0 0 ! Ind_BldPitch - The columns in OL_Filename that contains the blade pitch (1,2,3) inputs in rad [array] 0 ! Ind_GenTq - The column in OL_Filename that contains the generator torque in Nm @@ -162,6 +171,9 @@ 0.0000 0.0000 0.0000 0.0000 ! RP_Gains - PID gains and Tf of derivative for rotor position control (used if OL_Mode = 2) 0 ! Ind_CableControl - The column(s) in OL_Filename that contains the cable control inputs in m [Used with CC_Mode = 2, must be the same size as CC_Group_N] 0 ! Ind_StructControl - The column(s) in OL_Filename that contains the structural control inputs [Used with StC_Mode = 2, must be the same size as StC_Group_N] +0 ! Ind_R_Speed - Index (column, 1-indexed) of power rating via speed offset +0 ! Ind_R_Torque - Index (column, 1-indexed) of power rating via torque offset +0 ! Ind_R_Pitch - Index (column, 1-indexed) of power rating via pitch offset !------- Pitch Actuator Model ----------------------------------------------------- 3.140000000000 ! PA_CornerFreq - Pitch actuator bandwidth/cut-off frequency [rad/s] diff --git a/Examples/Test_Cases/NREL-5MW/DISCON.IN b/Examples/Test_Cases/NREL-5MW/DISCON.IN index bf86cf78..185c4126 100644 --- a/Examples/Test_Cases/NREL-5MW/DISCON.IN +++ b/Examples/Test_Cases/NREL-5MW/DISCON.IN @@ -1,5 +1,5 @@ ! Controller parameter input file for the NREL-5MW wind turbine -! - File written using ROSCO version 2.9.4 controller tuning logic on 07/01/24 +! - File written using ROSCO version 2.9.4 controller tuning logic on 09/03/24 !------- SIMULATION CONTROL ------------------------------------------------------------ 1 ! LoggingLevel - {0: write no debug files, 1: write standard output .dbg-file, 2: LoggingLevel 1 + ROSCO LocalVars (.dbg2) 3: LoggingLevel 2 + complete avrSWAP-array (.dbg3)} @@ -10,12 +10,12 @@ !------- CONTROLLER FLAGS ------------------------------------------------- 1 ! F_LPFType - (1: first-order low-pass filter, 2: second-order low-pass filter), [rad/s] (currently filters generator speed and pitch control signals 0 ! IPC_ControlMode - Turn Individual Pitch Control (IPC) for fatigue load reductions (pitch contribution) {0: off, 1: 1P reductions, 2: 1P+2P reductions} -3 ! VS_ControlMode - Generator torque control mode in above rated conditions (0- no torque control, 1- k*omega^2 with PI transitions, 2- WSE TSR Tracking, 3- Power-based TSR Tracking)} -0 ! VS_ConstPower - Do constant power torque control, where above rated torque varies, 0 for constant torque} +2 ! VS_ControlMode - Generator torque control mode in above rated conditions (0- no torque control, 1- k*omega^2 with PI transitions, 2- WSE TSR Tracking, 3- Power-based TSR Tracking)} +1 ! VS_ConstPower - Do constant power torque control, where above rated torque varies, 0 for constant torque} 1 ! PC_ControlMode - Blade pitch control mode {0: No pitch, fix to fine pitch, 1: active PI blade pitch control} 0 ! Y_ControlMode - Yaw control mode {0: no yaw control, 1: yaw rate control, 2: yaw-by-IPC} 1 ! SS_Mode - Setpoint Smoother mode {0: no setpoint smoothing, 1: introduce setpoint smoothing} -0 ! PRC_Mode - Power reference tracking mode{0: use standard rotor speed set points, 1: use PRC rotor speed setpoints} +0 ! PRC_Mode - Power reference tracking mode{0: power control disabled, 1: lookup table from wind speed to generator speed setpoints, 2: change speed, torque, pitch to control power} 2 ! WE_Mode - Wind speed estimator mode {0: One-second low pass filtered hub height wind speed, 1: Immersion and Invariance Estimator, 2: Extended Kalman Filter} 1 ! PS_Mode - Pitch saturation mode {0: no pitch saturation, 1: implement pitch saturation} 0 ! SD_Mode - Shutdown mode {0: no shutdown procedure, 1: pitch to max pitch at shutdown} @@ -53,7 +53,7 @@ !------- BLADE PITCH CONTROL ---------------------------------------------- 30 ! PC_GS_n - Amount of gain-scheduling table entries 0.056789 0.084492 0.106018 0.124332 0.140807 0.155903 0.169931 0.183270 0.196062 0.208354 0.220050 0.231503 0.242646 0.253377 0.263967 0.274233 0.284343 0.294292 0.303997 0.313626 0.322957 0.332260 0.341319 0.350368 0.359221 0.368059 0.376700 0.385301 0.393691 0.402050 ! PC_GS_angles - Gain-schedule table: pitch angles [rad]. --0.019085 -0.016739 -0.014837 -0.013265 -0.011943 -0.010816 -0.009844 -0.008997 -0.008252 -0.007593 -0.007004 -0.006475 -0.005998 -0.005565 -0.005171 -0.004810 -0.004479 -0.004173 -0.003890 -0.003628 -0.003385 -0.003157 -0.002945 -0.002746 -0.002559 -0.002384 -0.002219 -0.002062 -0.001915 -0.001775 ! PC_GS_KP - Gain-schedule table: pitch controller kp gains [s]. +-0.020751 -0.018231 -0.016188 -0.014498 -0.013078 -0.011868 -0.010823 -0.009913 -0.009113 -0.008404 -0.007772 -0.007204 -0.006692 -0.006227 -0.005803 -0.005415 -0.005059 -0.004731 -0.004427 -0.004146 -0.003884 -0.003640 -0.003411 -0.003198 -0.002997 -0.002809 -0.002631 -0.002463 -0.002305 -0.002155 ! PC_GS_KP - Gain-schedule table: pitch controller kp gains [s]. -0.008417 -0.007536 -0.006822 -0.006232 -0.005735 -0.005312 -0.004947 -0.004629 -0.004350 -0.004102 -0.003881 -0.003682 -0.003503 -0.003341 -0.003192 -0.003057 -0.002932 -0.002818 -0.002712 -0.002613 -0.002522 -0.002436 -0.002357 -0.002282 -0.002212 -0.002146 -0.002084 -0.002025 -0.001970 -0.001917 ! PC_GS_KI - Gain-schedule table: pitch controller ki gains [-]. 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 ! PC_GS_KD - Gain-schedule table: pitch controller kd gains 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 ! PC_GS_TF - Gain-schedule table: pitch controller tf gains (derivative filter) @@ -77,7 +77,7 @@ !------- VS TORQUE CONTROL ------------------------------------------------ 94.40000000000 ! VS_GenEff - Generator efficiency mechanical power -> electrical power, [should match the efficiency defined in the generator properties!], [%] 43093.51876000 ! VS_ArSatTq - Above rated generator torque PI control saturation, [Nm] -1500000.000000 ! VS_MaxRat - Maximum torque rate (in absolute value) in torque controller, [Nm/s]. +40000.00000000 ! VS_MaxRat - Maximum torque rate (in absolute value) in torque controller, [Nm/s]. 47402.87063000 ! VS_MaxTq - Maximum generator torque in Region 3 (HSS side), [Nm]. 0.000000000000 ! VS_MinTq - Minimum generator torque (HSS side), [Nm]. 35.29006000000 ! VS_MinOMSpd - Minimum generator speed [rad/s] @@ -96,6 +96,13 @@ 0.00100 ! SS_PCGain - Collective pitch controller setpoint smoother gain, [-]. !------- POWER REFERENCE TRACKING -------------------------------------- +0 ! PRC_Comm - Power reference communication mode when PRC_Mode = 2, 0- use constant DISCON inputs, 1- use open loop inputs, 2- use ZMQ inputs +1.00000 ! PRC_R_Torque - Constant power rating through changing the rated torque, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Speed - Constant power rating through changing the rated generator speed, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Pitch - Constant power rating through changing the fine pitch angle, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective below rated [-] +20 ! PRC_Table_n - Number of elements in PRC_R to _Pitch table. Used if PRC_Mode = 1. +0.0000 0.0526 0.1053 0.1579 0.2105 0.2632 0.3158 0.3684 0.4211 0.4737 0.5263 0.5789 0.6316 0.6842 0.7368 0.7895 0.8421 0.8947 0.9474 1.0000 ! PRC_R_Table - Table of turbine rating versus fine pitch (PRC_Pitch_Table), length should be PRC_Table_n, default is 1 [-]. Used if PRC_Mode = 1. +0.1933 0.1876 0.1819 0.1761 0.1701 0.1640 0.1577 0.1511 0.1446 0.1375 0.1303 0.1227 0.1147 0.1060 0.0969 0.0865 0.0750 0.0615 0.0436 0.0018 ! PRC_Pitch_Table - Table of fine pitch versus PRC_R_Table, length should be PRC_Table_n [rad]. Used if PRC_Mode = 1. 2 ! PRC_n - Number of elements in PRC_WindSpeeds and PRC_GenSpeeds array 0.07854 ! PRC_LPF_Freq - Frequency of the low pass filter on the wind speed estimate used to set PRC_GenSpeeds [rad/s] 3.0000 25.0000 ! PRC_WindSpeeds - Array of wind speeds used in rotor speed vs. wind speed lookup table [m/s] @@ -113,7 +120,7 @@ 36 26 ! PerfTableSize - Size of rotor performance tables, first number refers to number of blade pitch angles, second number referse to number of tip-speed ratios 60 ! WE_FOPoles_N - Number of first-order system poles used in EKF 3.0000 3.2897 3.5793 3.8690 4.1586 4.4483 4.7379 5.0276 5.3172 5.6069 5.8966 6.1862 6.4759 6.7655 7.0552 7.3448 7.6345 7.9241 8.2138 8.5034 8.7931 9.0828 9.3724 9.6621 9.9517 10.2414 10.5310 10.8207 11.1103 11.4000 11.8533 12.3067 12.7600 13.2133 13.6667 14.1200 14.5733 15.0267 15.4800 15.9333 16.3867 16.8400 17.2933 17.7467 18.2000 18.6533 19.1067 19.5600 20.0133 20.4667 20.9200 21.3733 21.8267 22.2800 22.7333 23.1867 23.6400 24.0933 24.5467 25.0000 ! WE_FOPoles_v - Wind speeds corresponding to first-order system poles [m/s] --0.01640352 -0.01798730 -0.01957109 -0.02115488 -0.02273867 -0.02432245 -0.02590624 -0.02749003 -0.02907382 -0.03065761 -0.03224139 -0.03382518 -0.03540897 -0.03699276 -0.03857654 -0.04016033 -0.04174412 -0.04332791 -0.04491170 -0.04649548 -0.04807927 -0.04966306 -0.05124685 -0.05283063 -0.05441442 -0.05599821 -0.05763347 -0.05907205 -0.06912889 -0.05993737 -0.05021616 -0.05780794 -0.06849544 -0.08052122 -0.09351145 -0.10710432 -0.12106730 -0.13547025 -0.15041833 -0.16588021 -0.18135551 -0.19724989 -0.21342206 -0.22970423 -0.24663982 -0.26354532 -0.28064419 -0.29794214 -0.31532544 -0.33332004 -0.35128130 -0.36985759 -0.38827549 -0.40713466 -0.42584061 -0.44509747 -0.46449375 -0.48460700 -0.50475242 -0.52537402 ! WE_FOPoles - First order system poles [1/s] +-0.01640352 -0.01798730 -0.01957109 -0.02115488 -0.02273867 -0.02432245 -0.02590624 -0.02749003 -0.02907382 -0.03065761 -0.03224139 -0.03382518 -0.03540897 -0.03699276 -0.03857654 -0.04016033 -0.04174412 -0.04332791 -0.04491170 -0.04649548 -0.04807927 -0.04966306 -0.05124685 -0.05283063 -0.05441442 -0.05599821 -0.05763347 -0.05907205 -0.06912889 -0.05993737 0.02104202 0.01345023 0.00276274 -0.00926304 -0.02225327 -0.03584614 -0.04980912 -0.06421207 -0.07916015 -0.09462203 -0.11009734 -0.12599172 -0.14216388 -0.15844606 -0.17538164 -0.19228714 -0.20938601 -0.22668396 -0.24406726 -0.26206186 -0.28002312 -0.29859942 -0.31701731 -0.33587649 -0.35458243 -0.37383930 -0.39323557 -0.41334882 -0.43349424 -0.45411584 ! WE_FOPoles - First order system poles [1/s] !------- YAW CONTROL ------------------------------------------------------ 0.00000 ! Y_uSwitch - Wind speed to switch between Y_ErrThresh. If zero, only the second value of Y_ErrThresh is used [m/s] @@ -154,6 +161,8 @@ !------- Open Loop Control ----------------------------------------------------- "unused" ! OL_Filename - Input file with open loop timeseries (absolute path or relative to this file) +0 ! OL_BP_Mode - Breakpoint mode for open loop control, 0 - indexed by time (default), 1 - indexed by wind speed] +0.000000 ! OL_BP_FiltFreq - Natural frequency of 1st order filter on breakpoint for open loop control. 0 will skip filter. 0 ! Ind_Breakpoint - The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1) 0 0 0 ! Ind_BldPitch - The columns in OL_Filename that contains the blade pitch (1,2,3) inputs in rad [array] 0 ! Ind_GenTq - The column in OL_Filename that contains the generator torque in Nm @@ -162,6 +171,9 @@ 0.0000 0.0000 0.0000 0.0000 ! RP_Gains - PID gains and Tf of derivative for rotor position control (used if OL_Mode = 2) 0 ! Ind_CableControl - The column(s) in OL_Filename that contains the cable control inputs in m [Used with CC_Mode = 2, must be the same size as CC_Group_N] 0 ! Ind_StructControl - The column(s) in OL_Filename that contains the structural control inputs [Used with StC_Mode = 2, must be the same size as StC_Group_N] +0 ! Ind_R_Speed - Index (column, 1-indexed) of power rating via speed offset +0 ! Ind_R_Torque - Index (column, 1-indexed) of power rating via torque offset +0 ! Ind_R_Pitch - Index (column, 1-indexed) of power rating via pitch offset !------- Pitch Actuator Model ----------------------------------------------------- 3.140000000000 ! PA_CornerFreq - Pitch actuator bandwidth/cut-off frequency [rad/s] diff --git a/Examples/Test_Cases/NREL_2p8_127/NREL-2p8-127_DISCON.IN b/Examples/Test_Cases/NREL_2p8_127/NREL-2p8-127_DISCON.IN index 46be3ce2..a73c2829 100644 --- a/Examples/Test_Cases/NREL_2p8_127/NREL-2p8-127_DISCON.IN +++ b/Examples/Test_Cases/NREL_2p8_127/NREL-2p8-127_DISCON.IN @@ -1,5 +1,5 @@ ! Controller parameter input file for the NREL-2p8-127 wind turbine -! - File written using ROSCO version 2.9.4 controller tuning logic on 07/01/24 +! - File written using ROSCO version 2.9.4 controller tuning logic on 09/03/24 !------- SIMULATION CONTROL ------------------------------------------------------------ 2 ! LoggingLevel - {0: write no debug files, 1: write standard output .dbg-file, 2: LoggingLevel 1 + ROSCO LocalVars (.dbg2) 3: LoggingLevel 2 + complete avrSWAP-array (.dbg3)} @@ -15,7 +15,7 @@ 1 ! PC_ControlMode - Blade pitch control mode {0: No pitch, fix to fine pitch, 1: active PI blade pitch control} 0 ! Y_ControlMode - Yaw control mode {0: no yaw control, 1: yaw rate control, 2: yaw-by-IPC} 1 ! SS_Mode - Setpoint Smoother mode {0: no setpoint smoothing, 1: introduce setpoint smoothing} -0 ! PRC_Mode - Power reference tracking mode{0: use standard rotor speed set points, 1: use PRC rotor speed setpoints} +0 ! PRC_Mode - Power reference tracking mode{0: power control disabled, 1: lookup table from wind speed to generator speed setpoints, 2: change speed, torque, pitch to control power} 2 ! WE_Mode - Wind speed estimator mode {0: One-second low pass filtered hub height wind speed, 1: Immersion and Invariance Estimator, 2: Extended Kalman Filter} 1 ! PS_Mode - Pitch saturation mode {0: no pitch saturation, 1: implement pitch saturation} 0 ! SD_Mode - Shutdown mode {0: no shutdown procedure, 1: pitch to max pitch at shutdown} @@ -77,7 +77,7 @@ !------- VS TORQUE CONTROL ------------------------------------------------ 93.94773000000 ! VS_GenEff - Generator efficiency mechanical power -> electrical power, [should match the efficiency defined in the generator properties!], [%] 24248.54567000 ! VS_ArSatTq - Above rated generator torque PI control saturation, [Nm] -1500000.000000 ! VS_MaxRat - Maximum torque rate (in absolute value) in torque controller, [Nm/s]. +22000.00000000 ! VS_MaxRat - Maximum torque rate (in absolute value) in torque controller, [Nm/s]. 26673.40024000 ! VS_MaxTq - Maximum generator torque in Region 3 (HSS side), [Nm]. 0.000000000000 ! VS_MinTq - Minimum generator torque (HSS side), [Nm]. 37.81562000000 ! VS_MinOMSpd - Minimum generator speed [rad/s] @@ -96,6 +96,13 @@ 0.00100 ! SS_PCGain - Collective pitch controller setpoint smoother gain, [-]. !------- POWER REFERENCE TRACKING -------------------------------------- +0 ! PRC_Comm - Power reference communication mode when PRC_Mode = 2, 0- use constant DISCON inputs, 1- use open loop inputs, 2- use ZMQ inputs +1.00000 ! PRC_R_Torque - Constant power rating through changing the rated torque, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Speed - Constant power rating through changing the rated generator speed, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +1.00000 ! PRC_R_Pitch - Constant power rating through changing the fine pitch angle, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective below rated [-] +20 ! PRC_Table_n - Number of elements in PRC_R to _Pitch table. Used if PRC_Mode = 1. +0.0000 0.0526 0.1053 0.1579 0.2105 0.2632 0.3158 0.3684 0.4211 0.4737 0.5263 0.5789 0.6316 0.6842 0.7368 0.7895 0.8421 0.8947 0.9474 1.0000 ! PRC_R_Table - Table of turbine rating versus fine pitch (PRC_Pitch_Table), length should be PRC_Table_n, default is 1 [-]. Used if PRC_Mode = 1. +0.2300 0.2236 0.2170 0.2102 0.2032 0.1960 0.1886 0.1808 0.1729 0.1646 0.1561 0.1471 0.1378 0.1280 0.1174 0.1060 0.0936 0.0791 0.0606 0.0000 ! PRC_Pitch_Table - Table of fine pitch versus PRC_R_Table, length should be PRC_Table_n [rad]. Used if PRC_Mode = 1. 2 ! PRC_n - Number of elements in PRC_WindSpeeds and PRC_GenSpeeds array 0.07854 ! PRC_LPF_Freq - Frequency of the low pass filter on the wind speed estimate used to set PRC_GenSpeeds [rad/s] 3.0000 25.0000 ! PRC_WindSpeeds - Array of wind speeds used in rotor speed vs. wind speed lookup table [m/s] @@ -154,6 +161,8 @@ !------- Open Loop Control ----------------------------------------------------- "unused" ! OL_Filename - Input file with open loop timeseries (absolute path or relative to this file) +0 ! OL_BP_Mode - Breakpoint mode for open loop control, 0 - indexed by time (default), 1 - indexed by wind speed] +0.000000 ! OL_BP_FiltFreq - Natural frequency of 1st order filter on breakpoint for open loop control. 0 will skip filter. 0 ! Ind_Breakpoint - The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1) 0 0 0 ! Ind_BldPitch - The columns in OL_Filename that contains the blade pitch (1,2,3) inputs in rad [array] 0 ! Ind_GenTq - The column in OL_Filename that contains the generator torque in Nm @@ -162,6 +171,9 @@ 0.0000 0.0000 0.0000 0.0000 ! RP_Gains - PID gains and Tf of derivative for rotor position control (used if OL_Mode = 2) 0 ! Ind_CableControl - The column(s) in OL_Filename that contains the cable control inputs in m [Used with CC_Mode = 2, must be the same size as CC_Group_N] 0 ! Ind_StructControl - The column(s) in OL_Filename that contains the structural control inputs [Used with StC_Mode = 2, must be the same size as StC_Group_N] +0 ! Ind_R_Speed - Index (column, 1-indexed) of power rating via speed offset +0 ! Ind_R_Torque - Index (column, 1-indexed) of power rating via torque offset +0 ! Ind_R_Pitch - Index (column, 1-indexed) of power rating via pitch offset !------- Pitch Actuator Model ----------------------------------------------------- 3.140000000000 ! PA_CornerFreq - Pitch actuator bandwidth/cut-off frequency [rad/s] diff --git a/Examples/Tune_Cases/NREL5MW.yaml b/Examples/Tune_Cases/NREL5MW.yaml index 1da5f641..51d9a0ac 100644 --- a/Examples/Tune_Cases/NREL5MW.yaml +++ b/Examples/Tune_Cases/NREL5MW.yaml @@ -29,7 +29,8 @@ controller_params: F_LPFType: 1 # {1: first-order low-pass filter, 2: second-order low-pass filter}, [rad/s] (currently filters generator speed and pitch control signals) F_NotchType: 0 # Notch filter on generator speed and/or tower fore-aft motion (for floating) {0: disable, 1: generator speed, 2: tower-top fore-aft motion, 3: generator speed and tower-top fore-aft motion} IPC_ControlMode: 0 # Turn Individual Pitch Control (IPC) for fatigue load reductions (pitch contribution) {0: off, 1: 1P reductions, 2: 1P+2P reductions} - VS_ControlMode: 3 # Generator torque control mode in above rated conditions {0: constant torque, 1: constant power, 2: TSR tracking PI control} + VS_ControlMode: 2 # Generator torque control mode in above rated conditions {0: constant torque, 1: constant power, 2: TSR tracking PI control} + VS_ConstPower: 1 # Use constant power PC_ControlMode: 1 # Blade pitch control mode {0: No pitch, fix to fine pitch, 1: active PI blade pitch control} Y_ControlMode: 0 # Yaw control mode {0: no yaw control, 1: yaw rate control, 2: yaw-by-IPC} SS_Mode: 1 # Setpoint Smoother mode {0: no setpoint smoothing, 1: introduce setpoint smoothing} diff --git a/Examples/Tune_Cases/NREL5MW_PassThrough.yaml b/Examples/Tune_Cases/NREL5MW_PassThrough.yaml index 143e4679..78e7f4ab 100644 --- a/Examples/Tune_Cases/NREL5MW_PassThrough.yaml +++ b/Examples/Tune_Cases/NREL5MW_PassThrough.yaml @@ -29,8 +29,8 @@ controller_params: F_LPFType: 1 # {1: first-order low-pass filter, 2: second-order low-pass filter}, [rad/s] (currently filters generator speed and pitch control signals) F_NotchType: 0 # Notch filter on generator speed and/or tower fore-aft motion (for floating) {0: disable, 1: generator speed, 2: tower-top fore-aft motion, 3: generator speed and tower-top fore-aft motion} IPC_ControlMode: 0 # Turn Individual Pitch Control (IPC) for fatigue load reductions (pitch contribution) {0: off, 1: 1P reductions, 2: 1P+2P reductions} - VS_ControlMode: 3 # Generator torque control mode in above rated conditions {0: constant torque, 1: constant power, 2: TSR tracking PI control} - PC_ControlMode: 1 # Blade pitch control mode {0: No pitch, fix to fine pitch, 1: active PI blade pitch control} + VS_ControlMode: 2 # Generator torque control mode in above rated conditions {0: constant torque, 1: constant power, 2: TSR tracking PI control} + VS_ConstPower: 1 # Use constant power PC_ControlMode: 1 # Blade pitch control mode {0: No pitch, fix to fine pitch, 1: active PI blade pitch control} Y_ControlMode: 0 # Yaw control mode {0: no yaw control, 1: yaw rate control, 2: yaw-by-IPC} SS_Mode: 1 # Setpoint Smoother mode {0: no setpoint smoothing, 1: introduce setpoint smoothing} WE_Mode: 2 # Wind speed estimator mode {0: One-second low pass filtered hub height wind speed, 1: Immersion and Invariance Estimator (Ortega et al.)} @@ -61,4 +61,4 @@ controller_params: PC_GS_KP: [-.02, -0.003] PC_GS_KI: [-.008, -.002] PC_GS_KD: [0, 0] - PC_GS_TF: [0, 0] \ No newline at end of file + PC_GS_TF: [0, 0] diff --git a/docs/images/06_MinPitch.png b/docs/images/06_MinPitch.png new file mode 100644 index 00000000..22bfce8a Binary files /dev/null and b/docs/images/06_MinPitch.png differ diff --git a/docs/images/29_AWC.png b/docs/images/29_AWC.png new file mode 100644 index 00000000..fab57eff Binary files /dev/null and b/docs/images/29_AWC.png differ diff --git a/docs/images/29_PRC_Methods.png b/docs/images/29_PRC_Methods.png new file mode 100644 index 00000000..8f2d4909 Binary files /dev/null and b/docs/images/29_PRC_Methods.png differ diff --git a/docs/images/29_Soft_Cutout.png b/docs/images/29_Soft_Cutout.png new file mode 100644 index 00000000..e323471c Binary files /dev/null and b/docs/images/29_Soft_Cutout.png differ diff --git a/docs/images/29_StartUp.png b/docs/images/29_StartUp.png new file mode 100644 index 00000000..f883ae94 Binary files /dev/null and b/docs/images/29_StartUp.png differ diff --git a/docs/source/api_change.rst b/docs/source/api_change.rst index 0127cd07..355af0c4 100644 --- a/docs/source/api_change.rst +++ b/docs/source/api_change.rst @@ -9,6 +9,34 @@ The changes are tabulated according to the line number, and flag name. The line number corresponds to the resulting line number after all changes are implemented. Thus, be sure to implement each in order so that subsequent line numbers are correct. +2.9.0 to develop +-------------------------- +**New power reference control mode** + +* De-rate or power boost the turbine using the reference speed, rated torque, or minimum pitch. More details can be found in the Examples page. + +====== ================= ====================================================================================================================================================================================================== +Changed in ROSCO develop +---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- +Line Input Name Example Value +====== ================= ====================================================================================================================================================================================================== +18 PRC_Mode 0 ! PRC_Mode - Power reference tracking mode{0: power control disabled, 1: lookup table from wind speed to generator speed setpoints, 2: change speed, torque, pitch to control power} +====== ================= ====================================================================================================================================================================================================== + +====== ======================= =============================================================================================================================================================================================================================================================== +New in ROSCO develop +------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- +Line Input Name Example Value +====== ======================= =============================================================================================================================================================================================================================================================== +99 PRC_Comm 0 ! PRC_Comm - Power reference communication mode when PRC_Mode = 2, 0- use constant DISCON inputs, 1- use open loop inputs, 2- use ZMQ inputs +100 PRC_R_Torque 1.00000 ! PRC_R_Torque - Constant power rating through changing the rated torque, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +101 PRC_R_Speed 1.00000 ! PRC_R_Speed - Constant power rating through changing the rated generator speed, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] +102 PRC_R_Pitch 1.00000 ! PRC_R_Pitch - Constant power rating through changing the fine pitch angle, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective below rated [-] +103 PRC_Table_n 20 ! PRC_Table_n - Number of elements in PRC_R to _Pitch table. Used if PRC_Mode = 1. +104 PRC_R_Table 0.0000 0.0526 0.1053 0.1579 0.2105 0.2632 0.3158 0.3684 0.4211 0.4737 0.5263 0.5789 0.6316 0.6842 0.7368 0.7895 0.8421 0.8947 0.9474 1.0000 ! PRC_R_Table - Table of turbine rating versus fine pitch (PRC_Pitch_Table), length should be PRC_Table_n, default is 1 [-]. Used if PRC_Mode = 1. +105 PRC_Pitch_Table 0.2296 0.2222 0.2144 0.2066 0.1984 0.1902 0.1814 0.1726 0.1633 0.1538 0.1439 0.1334 0.1226 0.1112 0.0989 0.0858 0.0715 0.0552 0.0351 0.0000 ! PRC_Pitch_Table - Table of fine pitch versus PRC_R_Table, length should be PRC_Table_n [rad]. Used if PRC_Mode = 1. +====== ======================= =============================================================================================================================================================================================================================================================== + 2.8.0 to 2.9.0 ------------------------------- **Flag to use exteneded Bladed Interface** @@ -48,7 +76,7 @@ Thus, be sure to implement each in order so that subsequent line numbers are cor * The set point is changed at a slow rate `TRA_RateLimit` to avoid generator power spikes. `VS_RefSpd`/100 is recommended. ====== ======================= =============================================================================================================================================================================================================================================================== -Removed in ROSCO develop +Removed in ROSCO 2.9.0 ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- Line Input Name Example Value ====== ======================= =============================================================================================================================================================================================================================================================== @@ -59,7 +87,7 @@ Line Input Name Example Value ====== ======================= =============================================================================================================================================================================================================================================================== -New in ROSCO develop +New in ROSCO 2.9.0 ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- Line Input Name Example Value ====== ======================= =============================================================================================================================================================================================================================================================== diff --git a/docs/source/examples.rst b/docs/source/examples.rst index ce1b73f1..da451271 100644 --- a/docs/source/examples.rst +++ b/docs/source/examples.rst @@ -129,5 +129,6 @@ A complete list of examples is given below: .. automodule:: 24_floating_feedback .. automodule:: 25_rotor_position_control .. automodule:: 26_marine_hydro -.. automodule:: 27_power_ref_control +.. automodule:: 27_soft_cut_out .. automodule:: 28_tower_resonance +.. automodule:: 29_power_control diff --git a/docs/source/figures/01_NREL5MW_Cp.png b/docs/source/figures/01_NREL5MW_Cp.png new file mode 100644 index 00000000..d3dcfa2c Binary files /dev/null and b/docs/source/figures/01_NREL5MW_Cp.png differ diff --git a/docs/source/figures/03_GainSched.png b/docs/source/figures/03_GainSched.png new file mode 100644 index 00000000..d9de5425 Binary files /dev/null and b/docs/source/figures/03_GainSched.png differ diff --git a/docs/source/figures/04_NREL5MW_SimpSim.png b/docs/source/figures/04_NREL5MW_SimpSim.png new file mode 100644 index 00000000..ef64a241 Binary files /dev/null and b/docs/source/figures/04_NREL5MW_SimpSim.png differ diff --git a/rosco/__init__.py b/rosco/__init__.py index c0bd0537..e0f06cc0 100644 --- a/rosco/__init__.py +++ b/rosco/__init__.py @@ -12,7 +12,7 @@ rosco_dir = os.path.dirname( os.path.abspath(__file__) ) libname = "libdiscon" -lib_path = [os.path.join(rosco_dir, "lib", libname+lib_ext), # pip installs (regular and editable) +lib_path = [os.path.join(rosco_dir, "lib", libname+lib_ext), # pip installs (regular and editable), `make install` also puts it here os.path.join(os.path.dirname( os.path.dirname( rosco_dir )), "local", "lib", libname+lib_ext), # WEIS library os.path.join(os.path.dirname( sysconfig.get_path('stdlib') ), libname+lib_ext), # conda installs os.path.join(os.path.dirname( sysconfig.get_path('stdlib') ), "lib", libname+lib_ext), # conda installs diff --git a/rosco/controller/CMakeLists.txt b/rosco/controller/CMakeLists.txt index 46705c25..2576d9dd 100644 --- a/rosco/controller/CMakeLists.txt +++ b/rosco/controller/CMakeLists.txt @@ -108,5 +108,5 @@ set(linuxDefault ${CMAKE_INSTALL_PREFIX} STREQUAL "/usr/local") set(windowsDefault ${CMAKE_INSTALL_PREFIX} STREQUAL "C:\\Program Files (x86)") if(${linuxDefault} OR ${windowsDefault}) message("TRUE") - set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/../../") + set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/../") endif() diff --git a/rosco/controller/rosco_registry/rosco_types.yaml b/rosco/controller/rosco_registry/rosco_types.yaml index 6f888f81..d1a2ff21 100644 --- a/rosco/controller/rosco_registry/rosco_types.yaml +++ b/rosco/controller/rosco_registry/rosco_types.yaml @@ -334,6 +334,9 @@ ControlParameters: PRC_Mode: <<: *integer description: Power reference tracking mode, 0- use standard rotor speed set points, 1- use PRC rotor speed setpoints + PRC_Comm: + <<: *integer + description: Power reference communication mode, 0- use constant DISCON inputs, 1- use open loop inputs, 2- use ZMQ inputs PRC_WindSpeeds: <<: *real description: Array of wind speeds used in rotor speed vs. wind speed lookup table @@ -348,6 +351,28 @@ ControlParameters: PRC_LPF_Freq: <<: *real description: Frequency of the low pass filter on the wind speed estimate used to set PRC_GenSpeeds [rad/s] + PRC_R_Torque: + <<: *real + description: Power rating through changing the rated torque, default is 1, effective above rated [-] + PRC_R_Speed: + <<: *real + description: Power rating through changing the rated generator speed, default is 1, effective above rated [-] + PRC_R_Pitch: + <<: *real + description: Power rating through changing the fine pitch angle, default is 1, effective below rated [-] + PRC_Table_n: + <<: *integer + description: Number of elements in PRC_R to _Pitch table + PRC_Pitch_Table: + <<: *real + allocatable: True + description: Table of fine pitch versus PRC_R_Table, length should be PRC_Table_n [rad] + PRC_R_Table: + <<: *real + allocatable: True + description: Table of turbine rating versus fine pitch (PRC_Pitch_Table), length should be PRC_Table_n, default is 1 [-] + + # Wind Speed Estimator WE_Mode: @@ -489,6 +514,12 @@ ControlParameters: OL_Mode: <<: *integer description: Open loop control mode {0 - no open loop control, 1 - open loop control vs. time, 2 - open loop control vs. wind speed} + OL_BP_Mode: + <<: *integer + description: Open loop control mode {0 - no open loop control, 1 - open loop control vs. time, 2 - open loop control vs. wind speed} + OL_BP_FiltFreq: + <<: *real + description: First order low pass filter cutoff frequency for open loop breakpoint Ind_Breakpoint: <<: *integer description: The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1) @@ -502,6 +533,15 @@ ControlParameters: Ind_YawRate: <<: *integer description: The column in OL_Filename that contains the generator torque in Nm + Ind_R_Speed: + <<: *integer + description: The column in OL_Filename that contains the R_Speed input + Ind_R_Torque: + <<: *integer + description: The column in OL_Filename that contains the R_Torque input + Ind_R_Pitch: + <<: *integer + description: The column in OL_Filename that contains the R_Pitch input Ind_Azimuth: <<: *integer description: The column in OL_Filename that contains the desired azimuth position in rad (used if OL_Mode = 2) @@ -553,6 +593,18 @@ ControlParameters: <<: *real allocatable: True description: Open loop azimuth timeseries + OL_R_Speed: + <<: *real + allocatable: True + description: Open loop R_Speed timeseries + OL_R_Torque: + <<: *real + allocatable: True + description: Open loop R_Torque timeseries + OL_R_Pitch: + <<: *real + allocatable: True + description: Open loop R_Pitch timeseries OL_Channels: <<: *real allocatable: True @@ -924,6 +976,9 @@ LocalVariables: iStatus: <<: *integer description: Initialization status + RestartWSE: + <<: *integer + description: Restart WSE flag, 0 - restart, 1- not, to mirror iStatus Time: <<: *real description: Time [s] @@ -957,9 +1012,15 @@ LocalVariables: NacVane: <<: *real description: Nacelle vane angle [deg] + NacVaneF: + <<: *real + description: Filtered nacelle vane angle [deg] HorWindV: <<: *real description: Hub height wind speed m/s + HorWindV_F: + <<: *real + description: Filtered hub height wind speed m/s rootMOOP: <<: *real description: Blade root bending moment [Nm] @@ -1077,9 +1138,6 @@ LocalVariables: PC_PitComT: <<: *real description: Total command pitch based on the sum of the proportional and integral terms [rad]. - PC_PitComT: - <<: *real - description: Collective pitch commmand from PI control [rad]. PC_PitComT_Last: <<: *real description: Last total command pitch based on the sum of the proportional and integral terms [rad]. @@ -1196,12 +1254,39 @@ LocalVariables: WE_VwIdot: <<: *real description: Differentiated integrated wind speed quantity for estimation [m/s] + WE_Op: + <<: *integer + description: WSE Operational state (0- not operating, 1-operating) + WE_Op_Last: + <<: *integer + description: Last WSE Operational state (0- not operating, 1-operating) VS_LastGenTrqF: <<: *real description: Differentiated integrated wind speed quantity for estimation [m/s] PRC_WSE_F: <<: *real description: Filtered wind speed estimate for power reference control + PRC_R_Speed: + <<: *real + description: Instantaneous PRC_R_Speed + PRC_R_Torque: + <<: *real + description: Instantaneous PRC_R_Torque + PRC_R_Pitch: + <<: *real + description: Instantaneous PRC_R_Pitch + PRC_R_Total: + <<: *real + description: Instantaneous PRC_R_Total + PRC_Min_Pitch: + <<: *real + description: Instantaneous PRC_Min_Pitch + PS_Min_Pitch: + <<: *real + description: Instantaneous peak shaving + OL_Index: + <<: *real + description: Open loop indexing variable (time or wind speed) SD: <<: *logical description: Shutdown, .FALSE. if inactive, .TRUE. if active @@ -1323,6 +1408,15 @@ LocalVariables: <<: *real size: 3 description: Pitch command offset provided by ZeroMQ + ZMQ_R_Speed: + <<: *real + description: R_Speed command provided by ZeroMQ + ZMQ_R_Torque: + <<: *real + description: R_Torque command provided by ZeroMQ + ZMQ_R_Pitch: + <<: *real + description: R_Pitch command provided by ZeroMQ WE: <<: *derived_type id: WE diff --git a/rosco/controller/rosco_registry/wfc_interface.yaml b/rosco/controller/rosco_registry/wfc_interface.yaml index 63b6e895..00d26051 100644 --- a/rosco/controller/rosco_registry/wfc_interface.yaml +++ b/rosco/controller/rosco_registry/wfc_interface.yaml @@ -24,4 +24,18 @@ setpoints: [ ZMQ_PitOffset(1), ZMQ_PitOffset(2), ZMQ_PitOffset(3), - ] \ No newline at end of file + ZMQ_R_Speed, + ZMQ_R_Torque, + ZMQ_R_Pitch, + ] + +setpoints_default: [ # order should match setpoints + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, +] \ No newline at end of file diff --git a/rosco/controller/rosco_registry/write_wfc_interface.py b/rosco/controller/rosco_registry/write_wfc_interface.py index 06895db8..fb26a493 100644 --- a/rosco/controller/rosco_registry/write_wfc_interface.py +++ b/rosco/controller/rosco_registry/write_wfc_interface.py @@ -40,6 +40,9 @@ def main(): if local_vars[ms]['size'] < ind_int: raise Exception(f'Requested index ({ind_int}) of WFC variable {ms} is greater than LocalVars version.') + # Check that length of defaults is the same as setpoints + if len(wfc_int['setpoints']) != len(wfc_int['setpoints_default']): + raise Exception(f'The lengths of setpoints and setpoints_default in the wfc_interface.yaml are not equal') write_zmq_f90(wfc_int) write_client_c(wfc_int) diff --git a/rosco/controller/src/Constants.f90 b/rosco/controller/src/Constants.f90 index 56298879..3a1ee0c2 100644 --- a/rosco/controller/src/Constants.f90 +++ b/rosco/controller/src/Constants.f90 @@ -37,4 +37,10 @@ MODULE Constants CHARACTER(1), PARAMETER :: Tab = CHAR( 9 ) + ! PRC Mode constants + INTEGER(IntKi), PARAMETER :: PRC_Comm_Constant = 0 ! Constant based on discon input + INTEGER(IntKi), PARAMETER :: PRC_Comm_OpenLoop = 1 ! Based on open loop input + INTEGER(IntKi), PARAMETER :: PRC_Comm_ZMQ = 2 ! Determined using ZMQ input + + END MODULE Constants diff --git a/rosco/controller/src/ControllerBlocks.f90 b/rosco/controller/src/ControllerBlocks.f90 index 986322c2..42bd6094 100644 --- a/rosco/controller/src/ControllerBlocks.f90 +++ b/rosco/controller/src/ControllerBlocks.f90 @@ -18,6 +18,7 @@ MODULE ControllerBlocks USE Constants USE Filters USE Functions +USE SysSubs IMPLICIT NONE @@ -35,14 +36,60 @@ SUBROUTINE ComputeVariablesSetpoints(CntrPar, LocalVar, objInst, DebugVar, ErrVa TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar - ! ----- Pitch controller speed and power error ----- + ! Set up power control + IF (CntrPar%PRC_Mode == 2) THEN ! Using power reference control + IF (CntrPar%PRC_Comm == PRC_Comm_Constant) THEN ! Constant, from DISCON + LocalVar%PRC_R_Speed = CntrPar%PRC_R_Speed + LocalVar%PRC_R_Torque = CntrPar%PRC_R_Torque + LocalVar%PRC_R_Pitch = CntrPar%PRC_R_Pitch + + ELSEIF (CntrPar%PRC_Comm == PRC_Comm_OpenLoop) THEN ! Open loop + + IF (CntrPar%Ind_R_Speed > 0) THEN + LocalVar%PRC_R_Speed = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_R_Speed,LocalVar%OL_Index,ErrVar) + WRITE(401,*) LocalVar%PRC_R_Speed + ELSE + LocalVar%PRC_R_Speed = 1.0_DbKi + ENDIF + + IF (CntrPar%Ind_R_Torque > 0) THEN + LocalVar%PRC_R_Torque = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_R_Torque,LocalVar%OL_Index,ErrVar) + ELSE + LocalVar%PRC_R_Torque = 1.0_DbKi + ENDIF + + IF (CntrPar%Ind_R_Pitch > 0) THEN + LocalVar%PRC_R_Pitch = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_R_Pitch,LocalVar%OL_Index,ErrVar) + ELSE + LocalVar%PRC_R_Pitch = 1.0_DbKi + ENDIF + + ELSEIF (CntrPar%PRC_Comm == PRC_Comm_ZMQ) THEN ! ZeroMQ + LocalVar%PRC_R_Speed = LocalVar%ZMQ_R_Speed + LocalVar%PRC_R_Torque = LocalVar%ZMQ_R_Torque + LocalVar%PRC_R_Pitch = LocalVar%ZMQ_R_Pitch + + ENDIF + + ! Set min pitch for power control, will be combined with peak shaving min pitch + LocalVar%PRC_Min_Pitch = interp1d(CntrPar%PRC_R_Table,CntrPar%PRC_Pitch_Table,LocalVar%PRC_R_Pitch, ErrVar) + + ELSE + LocalVar%PRC_R_Speed = 1.0_DbKi + LocalVar%PRC_R_Torque = 1.0_DbKi + LocalVar%PRC_R_Pitch = 1.0_DbKi + LocalVar%PRC_Min_Pitch = CntrPar%PC_FinePit + ENDIF + + ! End any power control before this point + + ! Change pitch reference speed + LocalVar%PC_RefSpd_PRC = CntrPar%PC_RefSpd * LocalVar%PRC_R_Speed - ! Power reference tracking generator speed + ! Lookup table for speed setpoint (PRC_Mode 1) IF (CntrPar%PRC_Mode == 1) THEN LocalVar%PRC_WSE_F = LPFilter(LocalVar%WE_Vw, LocalVar%DT,CntrPar%PRC_LPF_Freq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) LocalVar%PC_RefSpd_PRC = interp1d(CntrPar%PRC_WindSpeeds,CntrPar%PRC_GenSpeeds,LocalVar%PRC_WSE_F,ErrVar) - ELSE - LocalVar%PC_RefSpd_PRC = CntrPar%PC_RefSpd ENDIF ! Implement setpoint smoothing @@ -68,7 +115,8 @@ SUBROUTINE ComputeVariablesSetpoints(CntrPar, LocalVar, objInst, DebugVar, ErrVa LocalVar%VS_RefSpd_TSR = CntrPar%VS_RefSpd ENDIF - LocalVar%VS_RefSpd = LocalVar%VS_RefSpd_TSR + ! Change VS Ref speed based on R_Speed + LocalVar%VS_RefSpd = LocalVar%VS_RefSpd_TSR * LocalVar%PRC_R_Speed ! Exclude reference speeds specified by user IF (CntrPar%TRA_Mode > 0) THEN @@ -76,11 +124,11 @@ SUBROUTINE ComputeVariablesSetpoints(CntrPar, LocalVar, objInst, DebugVar, ErrVa END IF ! Saturate torque reference speed between min speed and rated speed - LocalVar%VS_RefSpd = saturate(LocalVar%VS_RefSpd,CntrPar%VS_MinOMSpd, CntrPar%VS_RefSpd) + LocalVar%VS_RefSpd = saturate(LocalVar%VS_RefSpd,CntrPar%VS_MinOMSpd, CntrPar%VS_RefSpd * LocalVar%PRC_R_Speed) - ! Implement power reference rotor speed (overwrites above), convert to generator speed + ! Simple lookup table for generator speed (PRC_Mode 1) IF (CntrPar%PRC_Mode == 1) THEN - LocalVar%VS_RefSpd = interp1d(CntrPar%PRC_WindSpeeds,CntrPar%PRC_GenSpeeds,LocalVar%WE_Vw_F,ErrVar) + LocalVar%VS_RefSpd = interp1d(CntrPar%PRC_WindSpeeds,CntrPar%PRC_GenSpeeds,LocalVar%PRC_WSE_F,ErrVar) ENDIF ! Implement setpoint smoothing @@ -226,30 +274,76 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er REAL(DbKi) :: WE_Inp_Pitch REAL(DbKi) :: WE_Inp_Torque REAL(DbKi) :: WE_Inp_Speed + REAL(DbKi) :: Max_Op_Pitch CHARACTER(*), PARAMETER :: RoutineName = 'WindSpeedEstimator' + CHARACTER(1024) :: WarningMessage - ! Saturate inputs to WSE + ! Saturate inputs to WSE: + ! Rotor speed IF (LocalVar%RotSpeedF < 0.25 * CntrPar%VS_MinOMSpd / CntrPar%WE_GearboxRatio) THEN WE_Inp_Speed = 0.25 * CntrPar%VS_MinOMSpd / CntrPar%WE_GearboxRatio + EPSILON(1.0_DbKi) ! If this is 0, could cause problems... ELSE WE_Inp_Speed = LocalVar%RotSpeedF END IF - IF (LocalVar%BlPitchCMeas < CntrPar%PC_MinPit) THEN - WE_Inp_Pitch = CntrPar%PC_MinPit + + ! Blade pitch + IF (CntrPar%WE_Mode > 0) THEN ! PerfData is only loaded if WE_Mode > 0 + Max_Op_Pitch = PerfData%Beta_vec(SIZE(PerfData%Beta_vec)) * D2R ! The Cp surface is only valid up to the end of Beta_vec ELSE - WE_Inp_Pitch = LocalVar%BlPitchCMeas - END IF + Max_Op_Pitch = 0.0_DbKi ! Doesn't matter if WE_Mode = 0 + ENDIF + + WE_Inp_Pitch = saturate(LocalVar%BlPitchCMeas, CntrPar%PC_MinPit,Max_Op_Pitch) + + ! Gen torque IF (LocalVar%VS_LastGenTrqF < 0.0001 * CntrPar%VS_RtTq) THEN WE_Inp_Torque = 0.0001 * CntrPar%VS_RtTq ELSE WE_Inp_Torque = LocalVar%VS_LastGenTrqF END IF + ! Check to see if in operational range + LocalVar%WE_Op_Last = LocalVar%WE_Op + IF (ABS(WE_Inp_Pitch - LocalVar%BlPitchCMeas) > 0) THEN + LocalVar%WE_Op = 0 + ELSEIF (ABS(WE_Inp_Torque - LocalVar%VS_LastGenTrqF) > 0) THEN + LocalVar%WE_Op = 0 + ELSEIF (ABS(WE_Inp_Speed - LocalVar%RotSpeedF) > 0) THEN + LocalVar%WE_Op = 0 + ELSE + LocalVar%WE_Op = 1 + ENDIF + + + ! Restart flag for WSE + LocalVar%RestartWSE = LocalVar%iStatus ! Same as iStatus by default + + IF (CntrPar%WE_Mode > 0) THEN + IF (LocalVar%WE_Op == 0 .AND. LocalVar%WE_Op_Last == 1) THEN ! Transition from operational to non-operational + WarningMessage = NewLine//'***************************************************************************************************************************************'//NewLine// & + 'ROSCO Warning: The wind speed estimator is used, but an input (pitch, rotor speed, or torque) has left the bounds of normal operation.'//NewLine// & + 'The filtered hub-height wind speed will be used instead. This warning will not persist even though the condition may.'//NewLine// & + 'Check WE_Op in the ROSCO .dbg file to see if the WSE is enabled (1) or disabled (0).'//NewLine// & + '***************************************************************************************************************************************' + PRINT *, TRIM(WarningMessage) + + LocalVar%RestartWSE = 0 ! Restart + ENDIF + + IF (LocalVar%WE_Op == 1 .AND. LocalVar%WE_Op_Last == 0) THEN ! Transition from non-operational to operational + LocalVar%RestartWSE = 0 ! Restart + ENDIF + ENDIF + + ! Filter the wind speed at hub height regardless, only use if WE_Mode = 0 or WE_Op = 0 + ! Re-initialize at WE_Vw if leaving operational wind, WE_Vw is initialized at HorWindV + LocalVar%HorWindV_F = cos(LocalVar%NacVaneF*D2R) * LPFilter(LocalVar%HorWindV, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%FP, LocalVar%RestartWSE, LocalVar%restart, objInst%instLPF, LocalVar%WE_Vw) + ! ---- Debug Inputs ------ DebugVar%WE_b = WE_Inp_Pitch DebugVar%WE_w = WE_Inp_Speed @@ -258,7 +352,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er ! ---- Define wind speed estimate ---- ! Inversion and Invariance Filter implementation - IF (CntrPar%WE_Mode == 1) THEN + IF (CntrPar%WE_Mode == 1 .AND. LocalVar%WE_Op > 0) THEN ! Compute AeroDynTorque Tau_r = AeroDynTorque(LocalVar%RotSpeedF, LocalVar%BlPitchCMeas, LocalVar, CntrPar, PerfData, ErrVar) @@ -267,7 +361,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er LocalVar%WE_Vw = LocalVar%WE_VwI + CntrPar%WE_Gamma*LocalVar%RotSpeedF ! Extended Kalman Filter (EKF) implementation - ELSEIF (CntrPar%WE_Mode == 2) THEN + ELSEIF (CntrPar%WE_Mode == 2 .AND. LocalVar%WE_Op > 0) THEN ! Define contant values L = 6.0 * CntrPar%WE_BladeRadius Ti = 0.18 @@ -276,12 +370,12 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er ! Define matrices to be filled F = RESHAPE((/0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0/),(/3,3/)) Q = RESHAPE((/0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0/),(/3,3/)) - IF (LocalVar%iStatus == 0) THEN + IF (LocalVar%RestartWSE == 0) THEN ! Initialize recurring values LocalVar%WE%om_r = WE_Inp_Speed LocalVar%WE%v_t = 0.0 - LocalVar%WE%v_m = max(LocalVar%HorWindV, 3.0_DbKi) ! avoid divide by 0 below if HorWindV is 0, which some AMRWind setups create - LocalVar%WE%v_h = max(LocalVar%HorWindV, 3.0_DbKi) ! avoid divide by 0 below if HorWindV is 0, which some AMRWind setups create + LocalVar%WE%v_m = max(LocalVar%WE_Vw, 3.0_DbKi) ! avoid divide by 0 below if WE_Vw is 0, which some AMRWind setups create + LocalVar%WE%v_h = max(LocalVar%WE_Vw, 3.0_DbKi) ! avoid divide by 0 below if WE_Vw is 0, which some AMRWind setups create lambda = WE_Inp_Speed * CntrPar%WE_BladeRadius/LocalVar%WE%v_h LocalVar%WE%xh = RESHAPE((/LocalVar%WE%om_r, LocalVar%WE%v_t, LocalVar%WE%v_m/),(/3,1/)) LocalVar%WE%P = RESHAPE((/0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 1.0/),(/3,3/)) @@ -349,8 +443,8 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er DebugVar%WE_Vt = LocalVar%WE%v_t DebugVar%WE_lambda = lambda ELSE - ! Filter wind speed at hub height as directly passed from OpenFAST - LocalVar%WE_Vw = LPFilter(LocalVar%HorWindV, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) + ! Use filtered hub-height + LocalVar%WE_Vw = LocalVar%HorWindV_F ENDIF DebugVar%WE_Vw = LocalVar%WE_Vw ! Add RoutineName to error message @@ -373,11 +467,13 @@ SUBROUTINE SetpointSmoother(LocalVar, CntrPar, objInst) TYPE(ObjectInstances), INTENT(INOUT) :: objInst ! Allocate Variables REAL(DbKi) :: DelOmega ! Reference generator speed shift, rad/s. + REAL(DbKi) :: R_Total ! Total power rating command ! ------ Setpoint Smoothing ------ IF ( CntrPar%SS_Mode == 1) THEN ! Find setpoint shift amount - DelOmega = ((LocalVar%PC_PitComT - LocalVar%PC_MinPit)/0.524) * CntrPar%SS_VSGain - ((CntrPar%VS_RtPwr - LocalVar%VS_LastGenPwr))/CntrPar%VS_RtPwr * CntrPar%SS_PCGain ! Normalize to 30 degrees for now + R_Total = LocalVar%PRC_R_Speed * LocalVar%PRC_R_Torque * LocalVar%PRC_R_Pitch + DelOmega = ((LocalVar%PC_PitComT - LocalVar%PC_MinPit)/0.524) * CntrPar%SS_VSGain - ((CntrPar%VS_RtPwr * R_Total - LocalVar%VS_LastGenPwr))/CntrPar%VS_RtPwr * CntrPar%SS_PCGain ! Normalize to 30 degrees for now DelOmega = DelOmega * CntrPar%PC_RefSpd ! Filter LocalVar%SS_DelOmegaF = LPFilter(DelOmega, LocalVar%DT, CntrPar%F_SSCornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) @@ -389,8 +485,9 @@ END SUBROUTINE SetpointSmoother !------------------------------------------------------------------------------------------------------------------------------- REAL(DbKi) FUNCTION PitchSaturation(LocalVar, CntrPar, objInst, DebugVar, ErrVar) ! PitchSaturation defines a minimum blade pitch angle based on a lookup table provided by DISCON.IN - ! SS_Mode = 0, No setpoint smoothing - ! SS_Mode = 1, Implement pitch saturation + ! Minimum pitch for power control also happens here + + USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, ObjectInstances, DebugVariables, ErrorVariables IMPLICIT NONE ! Inputs @@ -402,8 +499,11 @@ REAL(DbKi) FUNCTION PitchSaturation(LocalVar, CntrPar, objInst, DebugVar, ErrVar CHARACTER(*), PARAMETER :: RoutineName = 'PitchSaturation' - ! Define minimum blade pitch angle as a function of estimated wind speed - PitchSaturation = interp1d(CntrPar%PS_WindSpeeds, CntrPar%PS_BldPitchMin, LocalVar%WE_Vw_F, ErrVar) + ! Define minimum blade pitch angle for peak shaving as a function of estimated wind speed + LocalVar%PS_Min_Pitch = interp1d(CntrPar%PS_WindSpeeds, CntrPar%PS_BldPitchMin, LocalVar%WE_Vw_F, ErrVar) + + ! Total min pitch limit is greater of peak shaving and power control pitch + PitchSaturation = max(LocalVar%PS_Min_Pitch, LocalVar%PRC_Min_Pitch) ! Add RoutineName to error message IF (ErrVar%aviFAIL < 0) THEN diff --git a/rosco/controller/src/Controllers.f90 b/rosco/controller/src/Controllers.f90 index a32c391c..912a3706 100644 --- a/rosco/controller/src/Controllers.f90 +++ b/rosco/controller/src/Controllers.f90 @@ -122,15 +122,15 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar) IF (CntrPar%OL_Mode > 0) THEN IF (LocalVar%Time >= CntrPar%OL_Breakpoints(1)) THEN ! Time > first open loop breakpoint IF (CntrPar%Ind_BldPitch(1) > 0) THEN - LocalVar%PitCom(1) = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_BldPitch1,LocalVar%Time, ErrVar) + LocalVar%PitCom(1) = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_BldPitch1,LocalVar%OL_Index, ErrVar) ENDIF IF (CntrPar%Ind_BldPitch(2) > 0) THEN - LocalVar%PitCom(2) = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_BldPitch2,LocalVar%Time, ErrVar) + LocalVar%PitCom(2) = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_BldPitch2,LocalVar%OL_Index, ErrVar) ENDIF IF (CntrPar%Ind_BldPitch(3) > 0) THEN - LocalVar%PitCom(3) = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_BldPitch3,LocalVar%Time, ErrVar) + LocalVar%PitCom(3) = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_BldPitch3,LocalVar%OL_Index, ErrVar) ENDIF ENDIF ENDIF @@ -207,17 +207,16 @@ SUBROUTINE VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar) ! -------- Variable-Speed Torque Controller -------- ! Define max torque IF (LocalVar%VS_State == 4) THEN - LocalVar%VS_MaxTq = CntrPar%VS_RtTq + LocalVar%VS_MaxTq = CntrPar%VS_RtTq * LocalVar%PRC_R_Torque ELSE - ! VS_MaxTq = CntrPar%VS_MaxTq ! NJA: May want to boost max torque - LocalVar%VS_MaxTq = CntrPar%VS_RtTq + LocalVar%VS_MaxTq = CntrPar%VS_RtTq * LocalVar%PRC_R_Torque ENDIF ! Optimal Tip-Speed-Ratio tracking controller IF ((CntrPar%VS_ControlMode == 2) .OR. (CntrPar%VS_ControlMode == 3)) THEN ! Constant Power, update VS_MaxTq IF (CntrPar%VS_ConstPower == 1) THEN - LocalVar%VS_MaxTq = min((CntrPar%VS_RtPwr/(CntrPar%VS_GenEff/100.0))/LocalVar%GenSpeedF, CntrPar%VS_MaxTq) + LocalVar%VS_MaxTq = min((CntrPar%VS_RtPwr * LocalVar%PRC_R_Torque /(CntrPar%VS_GenEff/100.0))/LocalVar%GenSpeedF, CntrPar%VS_MaxTq) END IF ! PI controller @@ -265,7 +264,7 @@ SUBROUTINE VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar) IF ((CntrPar%OL_Mode > 0) .AND. (CntrPar%Ind_GenTq > 0)) THEN ! Get current OL GenTq, applies for OL_Mode 1 and 2 IF (LocalVar%Time >= CntrPar%OL_Breakpoints(1)) THEN - LocalVar%GenTq = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_GenTq,LocalVar%Time,ErrVar) + LocalVar%GenTq = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_GenTq,LocalVar%OL_Index,ErrVar) ENDIF ! Azimuth tracking control @@ -417,7 +416,7 @@ SUBROUTINE YawRateControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar) ! Open loop yaw rate control - control input in rad/s IF ((CntrPar%OL_Mode > 0) .AND. (CntrPar%Ind_YawRate > 0)) THEN IF (LocalVar%Time >= CntrPar%OL_Breakpoints(1)) THEN - avrSWAP(48) = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_YawRate,LocalVar%Time, ErrVar) + avrSWAP(48) = interp1d(CntrPar%OL_Breakpoints,CntrPar%OL_YawRate,LocalVar%OL_Index, ErrVar) ENDIF ENDIF @@ -871,5 +870,152 @@ SUBROUTINE StructuralControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar) ENDIF END SUBROUTINE StructuralControl +!------------------------------------------------------------------------------------------------------------------------------- + REAL(DbKi) FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, piP, reset, inst) + USE ROSCO_Types, ONLY : piParams + + ! PI controller, with output saturation + + IMPLICIT NONE + ! Allocate Inputs + REAL(DbKi), INTENT(IN) :: error + REAL(DbKi), INTENT(IN) :: kp + REAL(DbKi), INTENT(IN) :: ki + REAL(DbKi), INTENT(IN) :: minValue + REAL(DbKi), INTENT(IN) :: maxValue + REAL(DbKi), INTENT(IN) :: DT + INTEGER(IntKi), INTENT(INOUT) :: inst + REAL(DbKi), INTENT(IN) :: I0 + TYPE(piParams), INTENT(INOUT) :: piP + LOGICAL, INTENT(IN) :: reset + ! Allocate local variables + INTEGER(IntKi) :: i ! Counter for making arrays + REAL(DbKi) :: PTerm ! Proportional term + + ! Initialize persistent variables/arrays, and set inital condition for integrator term + IF (reset) THEN + piP%ITerm(inst) = I0 + piP%ITermLast(inst) = I0 + + PIController = I0 + ELSE + PTerm = kp*error + piP%ITerm(inst) = piP%ITerm(inst) + DT*ki*error + piP%ITerm(inst) = saturate(piP%ITerm(inst), minValue, maxValue) + PIController = saturate(PTerm + piP%ITerm(inst), minValue, maxValue) + + piP%ITermLast(inst) = piP%ITerm(inst) + END IF + inst = inst + 1 + + END FUNCTION PIController + + +!------------------------------------------------------------------------------------------------------------------------------- + REAL(DbKi) FUNCTION PIDController(error, kp, ki, kd, tf, minValue, maxValue, DT, I0, piP, reset, objInst, LocalVar) + USE ROSCO_Types, ONLY : piParams, LocalVariables, ObjectInstances + + ! PI controller, with output saturation + + IMPLICIT NONE + ! Allocate Inputs + REAL(DbKi), INTENT(IN) :: error + REAL(DbKi), INTENT(IN) :: kp + REAL(DbKi), INTENT(IN) :: ki + REAL(DbKi), INTENT(IN) :: kd + REAL(DbKi), INTENT(IN) :: tf + REAL(DbKi), INTENT(IN) :: minValue + REAL(DbKi), INTENT(IN) :: maxValue + REAL(DbKi), INTENT(IN) :: DT + TYPE(ObjectInstances), INTENT(INOUT) :: objInst ! all object instances (PI, filters used here) + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + + REAL(DbKi), INTENT(IN) :: I0 + TYPE(piParams), INTENT(INOUT) :: piP + LOGICAL, INTENT(IN) :: reset + + ! Allocate local variables + INTEGER(IntKi) :: i ! Counter for making arrays + REAL(DbKi) :: PTerm, DTerm ! Proportional, deriv. terms + REAL(DbKi) :: EFilt ! Filtered error for derivative + + ! Always filter error + EFilt = LPFilter(error, DT, tf, LocalVar%FP, LocalVar%iStatus, reset, objInst%instLPF) + + ! Initialize persistent variables/arrays, and set inital condition for integrator term + IF (reset) THEN + piP%ITerm(objInst%instPI) = I0 + piP%ITermLast(objInst%instPI) = I0 + piP%ELast(objInst%instPI) = 0.0_DbKi + PIDController = I0 + ELSE + ! Proportional + PTerm = kp*error + + ! Integrate and saturate + piP%ITerm(objInst%instPI) = piP%ITerm(objInst%instPI) + DT*ki*error + piP%ITerm(objInst%instPI) = saturate(piP%ITerm(objInst%instPI), minValue, maxValue) + + ! Derivative (filtered) + DTerm = kd * (EFilt - piP%ELast(objInst%instPI)) / DT + + ! Saturate all + PIDController = saturate(PTerm + piP%ITerm(objInst%instPI) + DTerm, minValue, maxValue) + + ! Save lasts + piP%ITermLast(objInst%instPI) = piP%ITerm(objInst%instPI) + piP%ELast(objInst%instPI) = EFilt + END IF + objInst%instPI = objInst%instPI + 1 + + END FUNCTION PIDController + +!------------------------------------------------------------------------------------------------------------------------------- + REAL(DbKi) FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, DT, I0, piP, reset, inst) + ! PI controller, with output saturation. + ! Added error2 term for additional integral control input + USE ROSCO_Types, ONLY : piParams + + IMPLICIT NONE + ! Allocate Inputs + REAL(DbKi), INTENT(IN) :: error + REAL(DbKi), INTENT(IN) :: error2 + REAL(DbKi), INTENT(IN) :: kp + REAL(DbKi), INTENT(IN) :: ki2 + REAL(DbKi), INTENT(IN) :: ki + REAL(DbKi), INTENT(IN) :: minValue + REAL(DbKi), INTENT(IN) :: maxValue + REAL(DbKi), INTENT(IN) :: DT + INTEGER(IntKi), INTENT(INOUT) :: inst + REAL(DbKi), INTENT(IN) :: I0 + TYPE(piParams), INTENT(INOUT) :: piP + LOGICAL, INTENT(IN) :: reset + ! Allocate local variables + INTEGER(IntKi) :: i ! Counter for making arrays + REAL(DbKi) :: PTerm ! Proportional term + + ! Initialize persistent variables/arrays, and set inital condition for integrator term + IF (reset) THEN + piP%ITerm(inst) = I0 + piP%ITermLast(inst) = I0 + piP%ITerm2(inst) = I0 + piP%ITermLast2(inst) = I0 + + PIIController = I0 + ELSE + PTerm = kp*error + piP%ITerm(inst) = piP%ITerm(inst) + DT*ki*error + piP%ITerm2(inst) = piP%ITerm2(inst) + DT*ki2*error2 + piP%ITerm(inst) = saturate(piP%ITerm(inst), minValue, maxValue) + piP%ITerm2(inst) = saturate(piP%ITerm2(inst), minValue, maxValue) + PIIController = PTerm + piP%ITerm(inst) + piP%ITerm2(inst) + PIIController = saturate(PIIController, minValue, maxValue) + + piP%ITermLast(inst) = piP%ITerm(inst) + END IF + inst = inst + 1 + + END FUNCTION PIIController + !------------------------------------------------------------------------------------------------------------------------------- END MODULE Controllers diff --git a/rosco/controller/src/Filters.f90 b/rosco/controller/src/Filters.f90 index 0358e7da..3e7d17f4 100644 --- a/rosco/controller/src/Filters.f90 +++ b/rosco/controller/src/Filters.f90 @@ -22,6 +22,7 @@ MODULE Filters !............................................................................................................................... USE Constants + USE Functions IMPLICIT NONE CONTAINS @@ -333,6 +334,8 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, DebugVar, objInst, ErrVar TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar INTEGER(IntKi) :: K ! Integer used to loop through turbine blades INTEGER(IntKi) :: n ! Integer used to loop through notch filters + REAL(DbKi) :: NacVaneCosF ! Time-filtered x-component of NacVane (deg) + REAL(DbKi) :: NacVaneSinF ! Time-filtered y-component of NacVane (deg) ! If there's an error, don't even try to run IF (ErrVar%aviFAIL < 0) THEN @@ -421,6 +424,11 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, DebugVar, objInst, ErrVar LocalVar%VS_LastGenTrqF = SecLPFilter(LocalVar%VS_LastGenTrq, LocalVar%DT, CntrPar%F_LPFCornerFreq, 0.7_DbKi, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) LocalVar%PC_PitComTF = SecLPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%F_LPFCornerFreq*0.25, 0.7_DbKi, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) + ! Wind vane signal + NacVaneCosF = LPFilter(cos(LocalVar%NacVane*D2R), LocalVar%DT, CntrPar%F_YawErr, LocalVar%FP, LocalVar%iStatus, .FALSE., objInst%instLPF) ! (-) + NacVaneSinF = LPFilter(sin(LocalVar%NacVane*D2R), LocalVar%DT, CntrPar%F_YawErr, LocalVar%FP, LocalVar%iStatus, .FALSE., objInst%instLPF) ! (-) + LocalVar%NacVaneF = wrap_180(atan2(NacVaneSinF, NacVaneCosF) * R2D) ! (deg) + ! Debug Variables DebugVar%GenSpeedF = LocalVar%GenSpeedF DebugVar%RotSpeedF = LocalVar%RotSpeedF diff --git a/rosco/controller/src/Functions.f90 b/rosco/controller/src/Functions.f90 index aa411ead..711d76d3 100644 --- a/rosco/controller/src/Functions.f90 +++ b/rosco/controller/src/Functions.f90 @@ -29,7 +29,6 @@ MODULE Functions USE Constants -USE Filters IMPLICIT NONE @@ -91,154 +90,7 @@ REAL(DbKi) FUNCTION ratelimit(inputSignal, minRate, maxRate, DT, reset, rlP, ins END FUNCTION ratelimit - !------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, piP, reset, inst) - USE ROSCO_Types, ONLY : piParams - ! PI controller, with output saturation - - IMPLICIT NONE - ! Allocate Inputs - REAL(DbKi), INTENT(IN) :: error - REAL(DbKi), INTENT(IN) :: kp - REAL(DbKi), INTENT(IN) :: ki - REAL(DbKi), INTENT(IN) :: minValue - REAL(DbKi), INTENT(IN) :: maxValue - REAL(DbKi), INTENT(IN) :: DT - INTEGER(IntKi), INTENT(INOUT) :: inst - REAL(DbKi), INTENT(IN) :: I0 - TYPE(piParams), INTENT(INOUT) :: piP - LOGICAL, INTENT(IN) :: reset - ! Allocate local variables - INTEGER(IntKi) :: i ! Counter for making arrays - REAL(DbKi) :: PTerm ! Proportional term - - ! Initialize persistent variables/arrays, and set inital condition for integrator term - IF (reset) THEN - piP%ITerm(inst) = I0 - piP%ITermLast(inst) = I0 - - PIController = I0 - ELSE - PTerm = kp*error - piP%ITerm(inst) = piP%ITerm(inst) + DT*ki*error - piP%ITerm(inst) = saturate(piP%ITerm(inst), minValue, maxValue) - PIController = saturate(PTerm + piP%ITerm(inst), minValue, maxValue) - - piP%ITermLast(inst) = piP%ITerm(inst) - END IF - inst = inst + 1 - - END FUNCTION PIController - - -!------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION PIDController(error, kp, ki, kd, tf, minValue, maxValue, DT, I0, piP, reset, objInst, LocalVar) - USE ROSCO_Types, ONLY : piParams, LocalVariables, ObjectInstances - - ! PI controller, with output saturation - - IMPLICIT NONE - ! Allocate Inputs - REAL(DbKi), INTENT(IN) :: error - REAL(DbKi), INTENT(IN) :: kp - REAL(DbKi), INTENT(IN) :: ki - REAL(DbKi), INTENT(IN) :: kd - REAL(DbKi), INTENT(IN) :: tf - REAL(DbKi), INTENT(IN) :: minValue - REAL(DbKi), INTENT(IN) :: maxValue - REAL(DbKi), INTENT(IN) :: DT - TYPE(ObjectInstances), INTENT(INOUT) :: objInst ! all object instances (PI, filters used here) - TYPE(LocalVariables), INTENT(INOUT) :: LocalVar - - REAL(DbKi), INTENT(IN) :: I0 - TYPE(piParams), INTENT(INOUT) :: piP - LOGICAL, INTENT(IN) :: reset - - ! Allocate local variables - INTEGER(IntKi) :: i ! Counter for making arrays - REAL(DbKi) :: PTerm, DTerm ! Proportional, deriv. terms - REAL(DbKi) :: EFilt ! Filtered error for derivative - - ! Always filter error - EFilt = LPFilter(error, DT, tf, LocalVar%FP, LocalVar%iStatus, reset, objInst%instLPF) - - ! Initialize persistent variables/arrays, and set inital condition for integrator term - IF (reset) THEN - piP%ITerm(objInst%instPI) = I0 - piP%ITermLast(objInst%instPI) = I0 - piP%ELast(objInst%instPI) = 0.0_DbKi - PIDController = I0 - ELSE - ! Proportional - PTerm = kp*error - - ! Integrate and saturate - piP%ITerm(objInst%instPI) = piP%ITerm(objInst%instPI) + DT*ki*error - piP%ITerm(objInst%instPI) = saturate(piP%ITerm(objInst%instPI), minValue, maxValue) - - ! Derivative (filtered) - DTerm = kd * (EFilt - piP%ELast(objInst%instPI)) / DT - - ! Saturate all - PIDController = saturate(PTerm + piP%ITerm(objInst%instPI) + DTerm, minValue, maxValue) - - ! Save lasts - piP%ITermLast(objInst%instPI) = piP%ITerm(objInst%instPI) - piP%ELast(objInst%instPI) = EFilt - END IF - objInst%instPI = objInst%instPI + 1 - - END FUNCTION PIDController - -!------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, DT, I0, piP, reset, inst) - ! PI controller, with output saturation. - ! Added error2 term for additional integral control input - USE ROSCO_Types, ONLY : piParams - - IMPLICIT NONE - ! Allocate Inputs - REAL(DbKi), INTENT(IN) :: error - REAL(DbKi), INTENT(IN) :: error2 - REAL(DbKi), INTENT(IN) :: kp - REAL(DbKi), INTENT(IN) :: ki2 - REAL(DbKi), INTENT(IN) :: ki - REAL(DbKi), INTENT(IN) :: minValue - REAL(DbKi), INTENT(IN) :: maxValue - REAL(DbKi), INTENT(IN) :: DT - INTEGER(IntKi), INTENT(INOUT) :: inst - REAL(DbKi), INTENT(IN) :: I0 - TYPE(piParams), INTENT(INOUT) :: piP - LOGICAL, INTENT(IN) :: reset - ! Allocate local variables - INTEGER(IntKi) :: i ! Counter for making arrays - REAL(DbKi) :: PTerm ! Proportional term - - ! Initialize persistent variables/arrays, and set inital condition for integrator term - IF (reset) THEN - piP%ITerm(inst) = I0 - piP%ITermLast(inst) = I0 - piP%ITerm2(inst) = I0 - piP%ITermLast2(inst) = I0 - - PIIController = I0 - ELSE - PTerm = kp*error - piP%ITerm(inst) = piP%ITerm(inst) + DT*ki*error - piP%ITerm2(inst) = piP%ITerm2(inst) + DT*ki2*error2 - piP%ITerm(inst) = saturate(piP%ITerm(inst), minValue, maxValue) - piP%ITerm2(inst) = saturate(piP%ITerm2(inst), minValue, maxValue) - PIIController = PTerm + piP%ITerm(inst) + piP%ITerm2(inst) - PIIController = saturate(PIIController, minValue, maxValue) - - piP%ITermLast(inst) = piP%ITerm(inst) - END IF - inst = inst + 1 - - END FUNCTION PIIController - -!------------------------------------------------------------------------------------------------------------------------------- REAL(DbKi) FUNCTION interp1d(xData, yData, xq, ErrVar) ! interp1d 1-D interpolation (table lookup), xData should be strictly increasing diff --git a/rosco/controller/src/ROSCO_IO.f90 b/rosco/controller/src/ROSCO_IO.f90 index c537f9b4..33b2bddc 100644 --- a/rosco/controller/src/ROSCO_IO.f90 +++ b/rosco/controller/src/ROSCO_IO.f90 @@ -1,5 +1,5 @@ ! ROSCO IO -! This file is automatically generated by write_registry.py using ROSCO v2.9.0 +! This file is automatically generated by write_registry.py using ROSCO v2.9.4 ! For any modification to the registry, please edit the rosco_types.yaml accordingly MODULE ROSCO_IO @@ -37,6 +37,7 @@ SUBROUTINE WriteRestartFile(LocalVar, CntrPar, ErrVar, objInst, RootName, size_a ELSE WRITE( Un, IOSTAT=ErrStat) LocalVar%iStatus + WRITE( Un, IOSTAT=ErrStat) LocalVar%RestartWSE WRITE( Un, IOSTAT=ErrStat) LocalVar%Time WRITE( Un, IOSTAT=ErrStat) LocalVar%DT WRITE( Un, IOSTAT=ErrStat) LocalVar%WriteThisStep @@ -48,7 +49,9 @@ SUBROUTINE WriteRestartFile(LocalVar, CntrPar, ErrVar, objInst, RootName, size_a WRITE( Un, IOSTAT=ErrStat) LocalVar%RotSpeed WRITE( Un, IOSTAT=ErrStat) LocalVar%NacHeading WRITE( Un, IOSTAT=ErrStat) LocalVar%NacVane + WRITE( Un, IOSTAT=ErrStat) LocalVar%NacVaneF WRITE( Un, IOSTAT=ErrStat) LocalVar%HorWindV + WRITE( Un, IOSTAT=ErrStat) LocalVar%HorWindV_F WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(1) WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(2) WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(3) @@ -142,8 +145,17 @@ SUBROUTINE WriteRestartFile(LocalVar, CntrPar, ErrVar, objInst, RootName, size_a WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_Vw_F WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_VwI WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_VwIdot + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_Op + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_Op_Last WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrqF WRITE( Un, IOSTAT=ErrStat) LocalVar%PRC_WSE_F + WRITE( Un, IOSTAT=ErrStat) LocalVar%PRC_R_Speed + WRITE( Un, IOSTAT=ErrStat) LocalVar%PRC_R_Torque + WRITE( Un, IOSTAT=ErrStat) LocalVar%PRC_R_Pitch + WRITE( Un, IOSTAT=ErrStat) LocalVar%PRC_R_Total + WRITE( Un, IOSTAT=ErrStat) LocalVar%PRC_Min_Pitch + WRITE( Un, IOSTAT=ErrStat) LocalVar%PS_Min_Pitch + WRITE( Un, IOSTAT=ErrStat) LocalVar%OL_Index WRITE( Un, IOSTAT=ErrStat) LocalVar%SD WRITE( Un, IOSTAT=ErrStat) LocalVar%Fl_PitCom WRITE( Un, IOSTAT=ErrStat) LocalVar%NACIMU_FA_AccF @@ -235,6 +247,9 @@ SUBROUTINE WriteRestartFile(LocalVar, CntrPar, ErrVar, objInst, RootName, size_a WRITE( Un, IOSTAT=ErrStat) LocalVar%ZMQ_PitOffset(1) WRITE( Un, IOSTAT=ErrStat) LocalVar%ZMQ_PitOffset(2) WRITE( Un, IOSTAT=ErrStat) LocalVar%ZMQ_PitOffset(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%ZMQ_R_Speed + WRITE( Un, IOSTAT=ErrStat) LocalVar%ZMQ_R_Torque + WRITE( Un, IOSTAT=ErrStat) LocalVar%ZMQ_R_Pitch WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%om_r WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_t WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_m @@ -339,6 +354,7 @@ SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootNa ELSE READ( Un, IOSTAT=ErrStat) LocalVar%iStatus + READ( Un, IOSTAT=ErrStat) LocalVar%RestartWSE READ( Un, IOSTAT=ErrStat) LocalVar%Time READ( Un, IOSTAT=ErrStat) LocalVar%DT READ( Un, IOSTAT=ErrStat) LocalVar%WriteThisStep @@ -350,7 +366,9 @@ SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootNa READ( Un, IOSTAT=ErrStat) LocalVar%RotSpeed READ( Un, IOSTAT=ErrStat) LocalVar%NacHeading READ( Un, IOSTAT=ErrStat) LocalVar%NacVane + READ( Un, IOSTAT=ErrStat) LocalVar%NacVaneF READ( Un, IOSTAT=ErrStat) LocalVar%HorWindV + READ( Un, IOSTAT=ErrStat) LocalVar%HorWindV_F READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(1) READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(2) READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(3) @@ -444,8 +462,17 @@ SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootNa READ( Un, IOSTAT=ErrStat) LocalVar%WE_Vw_F READ( Un, IOSTAT=ErrStat) LocalVar%WE_VwI READ( Un, IOSTAT=ErrStat) LocalVar%WE_VwIdot + READ( Un, IOSTAT=ErrStat) LocalVar%WE_Op + READ( Un, IOSTAT=ErrStat) LocalVar%WE_Op_Last READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrqF READ( Un, IOSTAT=ErrStat) LocalVar%PRC_WSE_F + READ( Un, IOSTAT=ErrStat) LocalVar%PRC_R_Speed + READ( Un, IOSTAT=ErrStat) LocalVar%PRC_R_Torque + READ( Un, IOSTAT=ErrStat) LocalVar%PRC_R_Pitch + READ( Un, IOSTAT=ErrStat) LocalVar%PRC_R_Total + READ( Un, IOSTAT=ErrStat) LocalVar%PRC_Min_Pitch + READ( Un, IOSTAT=ErrStat) LocalVar%PS_Min_Pitch + READ( Un, IOSTAT=ErrStat) LocalVar%OL_Index READ( Un, IOSTAT=ErrStat) LocalVar%SD READ( Un, IOSTAT=ErrStat) LocalVar%Fl_PitCom READ( Un, IOSTAT=ErrStat) LocalVar%NACIMU_FA_AccF @@ -538,6 +565,9 @@ SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootNa READ( Un, IOSTAT=ErrStat) LocalVar%ZMQ_PitOffset(1) READ( Un, IOSTAT=ErrStat) LocalVar%ZMQ_PitOffset(2) READ( Un, IOSTAT=ErrStat) LocalVar%ZMQ_PitOffset(3) + READ( Un, IOSTAT=ErrStat) LocalVar%ZMQ_R_Speed + READ( Un, IOSTAT=ErrStat) LocalVar%ZMQ_R_Torque + READ( Un, IOSTAT=ErrStat) LocalVar%ZMQ_R_Pitch READ( Un, IOSTAT=ErrStat) LocalVar%WE%om_r READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_t READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_m @@ -692,158 +722,176 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, ErrVar, avrSWAP, RootName, size_av '[N/A]', '[N/A]', '[N/A]', '[N/A]', '[rad/s]', & '[deg]', '[deg]', '[deg]', '[N/A]', '[rad/s]', & '[rad/s]'] - nLocalVars = 124 + nLocalVars = 139 Allocate(LocalVarOutData(nLocalVars)) Allocate(LocalVarOutStrings(nLocalVars)) LocalVarOutData(1) = LocalVar%iStatus - LocalVarOutData(2) = LocalVar%Time - LocalVarOutData(3) = LocalVar%DT - LocalVarOutData(4) = LocalVar%n_DT - LocalVarOutData(5) = LocalVar%Time_Last - LocalVarOutData(6) = LocalVar%VS_GenPwr - LocalVarOutData(7) = LocalVar%VS_GenPwrF - LocalVarOutData(8) = LocalVar%GenSpeed - LocalVarOutData(9) = LocalVar%RotSpeed - LocalVarOutData(10) = LocalVar%NacHeading - LocalVarOutData(11) = LocalVar%NacVane - LocalVarOutData(12) = LocalVar%HorWindV - LocalVarOutData(13) = LocalVar%rootMOOP(1) - LocalVarOutData(14) = LocalVar%rootMOOPF(1) - LocalVarOutData(15) = LocalVar%BlPitch(1) - LocalVarOutData(16) = LocalVar%BlPitchCMeas - LocalVarOutData(17) = LocalVar%Azimuth - LocalVarOutData(18) = LocalVar%OL_Azimuth - LocalVarOutData(19) = LocalVar%AzUnwrapped - LocalVarOutData(20) = LocalVar%AzError - LocalVarOutData(21) = LocalVar%GenTqAz - LocalVarOutData(22) = LocalVar%AzBuffer(1) - LocalVarOutData(23) = LocalVar%NumBl - LocalVarOutData(24) = LocalVar%FA_Acc - LocalVarOutData(25) = LocalVar%NacIMU_FA_Acc - LocalVarOutData(26) = LocalVar%FA_AccHPF - LocalVarOutData(27) = LocalVar%FA_AccHPFI - LocalVarOutData(28) = LocalVar%FA_PitCom(1) - LocalVarOutData(29) = LocalVar%VS_RefSpd - LocalVarOutData(30) = LocalVar%VS_RefSpd_TSR - LocalVarOutData(31) = LocalVar%VS_RefSpd_TRA - LocalVarOutData(32) = LocalVar%VS_RefSpd_RL - LocalVarOutData(33) = LocalVar%PC_RefSpd - LocalVarOutData(34) = LocalVar%PC_RefSpd_SS - LocalVarOutData(35) = LocalVar%PC_RefSpd_PRC - LocalVarOutData(36) = LocalVar%RotSpeedF - LocalVarOutData(37) = LocalVar%GenSpeedF - LocalVarOutData(38) = LocalVar%GenTq - LocalVarOutData(39) = LocalVar%GenTqMeas - LocalVarOutData(40) = LocalVar%GenArTq - LocalVarOutData(41) = LocalVar%GenBrTq - LocalVarOutData(42) = LocalVar%IPC_PitComF(1) - LocalVarOutData(43) = LocalVar%PC_KP - LocalVarOutData(44) = LocalVar%PC_KI - LocalVarOutData(45) = LocalVar%PC_KD - LocalVarOutData(46) = LocalVar%PC_TF - LocalVarOutData(47) = LocalVar%PC_MaxPit - LocalVarOutData(48) = LocalVar%PC_MinPit - LocalVarOutData(49) = LocalVar%PC_PitComT - LocalVarOutData(50) = LocalVar%PC_PitComT_Last - LocalVarOutData(51) = LocalVar%PC_PitComTF - LocalVarOutData(52) = LocalVar%PC_PitComT_IPC(1) - LocalVarOutData(53) = LocalVar%PC_PwrErr - LocalVarOutData(54) = LocalVar%PC_SpdErr - LocalVarOutData(55) = LocalVar%IPC_AxisTilt_1P - LocalVarOutData(56) = LocalVar%IPC_AxisYaw_1P - LocalVarOutData(57) = LocalVar%IPC_AxisTilt_2P - LocalVarOutData(58) = LocalVar%IPC_AxisYaw_2P - LocalVarOutData(59) = LocalVar%axisTilt_1P - LocalVarOutData(60) = LocalVar%axisYaw_1P - LocalVarOutData(61) = LocalVar%axisYawF_1P - LocalVarOutData(62) = LocalVar%axisTilt_2P - LocalVarOutData(63) = LocalVar%axisYaw_2P - LocalVarOutData(64) = LocalVar%axisYawF_2P - LocalVarOutData(65) = LocalVar%IPC_KI(1) - LocalVarOutData(66) = LocalVar%IPC_KP(1) - LocalVarOutData(67) = LocalVar%IPC_IntSat - LocalVarOutData(68) = LocalVar%PC_State - LocalVarOutData(69) = LocalVar%PitCom(1) - LocalVarOutData(70) = LocalVar%PitComAct(1) - LocalVarOutData(71) = LocalVar%SS_DelOmegaF - LocalVarOutData(72) = LocalVar%TestType - LocalVarOutData(73) = LocalVar%Kp_Float - LocalVarOutData(74) = LocalVar%VS_MaxTq - LocalVarOutData(75) = LocalVar%VS_LastGenTrq - LocalVarOutData(76) = LocalVar%VS_LastGenPwr - LocalVarOutData(77) = LocalVar%VS_MechGenPwr - LocalVarOutData(78) = LocalVar%VS_SpdErrAr - LocalVarOutData(79) = LocalVar%VS_SpdErrBr - LocalVarOutData(80) = LocalVar%VS_SpdErr - LocalVarOutData(81) = LocalVar%VS_State - LocalVarOutData(82) = LocalVar%VS_Rgn3Pitch - LocalVarOutData(83) = LocalVar%WE_Vw - LocalVarOutData(84) = LocalVar%WE_Vw_F - LocalVarOutData(85) = LocalVar%WE_VwI - LocalVarOutData(86) = LocalVar%WE_VwIdot - LocalVarOutData(87) = LocalVar%VS_LastGenTrqF - LocalVarOutData(88) = LocalVar%PRC_WSE_F - LocalVarOutData(89) = LocalVar%Fl_PitCom - LocalVarOutData(90) = LocalVar%NACIMU_FA_AccF - LocalVarOutData(91) = LocalVar%FA_AccF - LocalVarOutData(92) = LocalVar%FA_Hist - LocalVarOutData(93) = LocalVar%TRA_LastRefSpd - LocalVarOutData(94) = LocalVar%VS_RefSpeed - LocalVarOutData(95) = LocalVar%PtfmTDX - LocalVarOutData(96) = LocalVar%PtfmTDY - LocalVarOutData(97) = LocalVar%PtfmTDZ - LocalVarOutData(98) = LocalVar%PtfmRDX - LocalVarOutData(99) = LocalVar%PtfmRDY - LocalVarOutData(100) = LocalVar%PtfmRDZ - LocalVarOutData(101) = LocalVar%PtfmTVX - LocalVarOutData(102) = LocalVar%PtfmTVY - LocalVarOutData(103) = LocalVar%PtfmTVZ - LocalVarOutData(104) = LocalVar%PtfmRVX - LocalVarOutData(105) = LocalVar%PtfmRVY - LocalVarOutData(106) = LocalVar%PtfmRVZ - LocalVarOutData(107) = LocalVar%PtfmTAX - LocalVarOutData(108) = LocalVar%PtfmTAY - LocalVarOutData(109) = LocalVar%PtfmTAZ - LocalVarOutData(110) = LocalVar%PtfmRAX - LocalVarOutData(111) = LocalVar%PtfmRAY - LocalVarOutData(112) = LocalVar%PtfmRAZ - LocalVarOutData(113) = LocalVar%CC_DesiredL(1) - LocalVarOutData(114) = LocalVar%CC_ActuatedL(1) - LocalVarOutData(115) = LocalVar%CC_ActuatedDL(1) - LocalVarOutData(116) = LocalVar%StC_Input(1) - LocalVarOutData(117) = LocalVar%Flp_Angle(1) - LocalVarOutData(118) = LocalVar%RootMyb_Last(1) - LocalVarOutData(119) = LocalVar%ACC_INFILE_SIZE - LocalVarOutData(120) = LocalVar%AWC_complexangle(1) - LocalVarOutData(121) = LocalVar%ZMQ_ID - LocalVarOutData(122) = LocalVar%ZMQ_YawOffset - LocalVarOutData(123) = LocalVar%ZMQ_TorqueOffset - LocalVarOutData(124) = LocalVar%ZMQ_PitOffset(1) - LocalVarOutStrings = [CHARACTER(15) :: 'iStatus', 'Time', 'DT', 'n_DT', 'Time_Last', & - 'VS_GenPwr', 'VS_GenPwrF', 'GenSpeed', 'RotSpeed', 'NacHeading', & - 'NacVane', 'HorWindV', 'rootMOOP', 'rootMOOPF', 'BlPitch', & - 'BlPitchCMeas', 'Azimuth', 'OL_Azimuth', 'AzUnwrapped', 'AzError', & - 'GenTqAz', 'AzBuffer', 'NumBl', 'FA_Acc', 'NacIMU_FA_Acc', & - 'FA_AccHPF', 'FA_AccHPFI', 'FA_PitCom', 'VS_RefSpd', 'VS_RefSpd_TSR', & - 'VS_RefSpd_TRA', 'VS_RefSpd_RL', 'PC_RefSpd', 'PC_RefSpd_SS', 'PC_RefSpd_PRC', & - 'RotSpeedF', 'GenSpeedF', 'GenTq', 'GenTqMeas', 'GenArTq', & - 'GenBrTq', 'IPC_PitComF', 'PC_KP', 'PC_KI', 'PC_KD', & - 'PC_TF', 'PC_MaxPit', 'PC_MinPit', 'PC_PitComT', 'PC_PitComT_Last', & - 'PC_PitComTF', 'PC_PitComT_IPC', 'PC_PwrErr', 'PC_SpdErr', 'IPC_AxisTilt_1P', & - 'IPC_AxisYaw_1P', 'IPC_AxisTilt_2P', 'IPC_AxisYaw_2P', 'axisTilt_1P', 'axisYaw_1P', & - 'axisYawF_1P', 'axisTilt_2P', 'axisYaw_2P', 'axisYawF_2P', 'IPC_KI', & - 'IPC_KP', 'IPC_IntSat', 'PC_State', 'PitCom', 'PitComAct', & - 'SS_DelOmegaF', 'TestType', 'Kp_Float', 'VS_MaxTq', 'VS_LastGenTrq', & - 'VS_LastGenPwr', 'VS_MechGenPwr', 'VS_SpdErrAr', 'VS_SpdErrBr', 'VS_SpdErr', & - 'VS_State', 'VS_Rgn3Pitch', 'WE_Vw', 'WE_Vw_F', 'WE_VwI', & - 'WE_VwIdot', 'VS_LastGenTrqF', 'PRC_WSE_F', 'Fl_PitCom', 'NACIMU_FA_AccF', & - 'FA_AccF', 'FA_Hist', 'TRA_LastRefSpd', 'VS_RefSpeed', 'PtfmTDX', & - 'PtfmTDY', 'PtfmTDZ', 'PtfmRDX', 'PtfmRDY', 'PtfmRDZ', & - 'PtfmTVX', 'PtfmTVY', 'PtfmTVZ', 'PtfmRVX', 'PtfmRVY', & - 'PtfmRVZ', 'PtfmTAX', 'PtfmTAY', 'PtfmTAZ', 'PtfmRAX', & - 'PtfmRAY', 'PtfmRAZ', 'CC_DesiredL', 'CC_ActuatedL', 'CC_ActuatedDL', & - 'StC_Input', 'Flp_Angle', 'RootMyb_Last', 'ACC_INFILE_SIZE', 'AWC_complexangle', & - 'ZMQ_ID', 'ZMQ_YawOffset', 'ZMQ_TorqueOffset', 'ZMQ_PitOffset'] + LocalVarOutData(2) = LocalVar%RestartWSE + LocalVarOutData(3) = LocalVar%Time + LocalVarOutData(4) = LocalVar%DT + LocalVarOutData(5) = LocalVar%n_DT + LocalVarOutData(6) = LocalVar%Time_Last + LocalVarOutData(7) = LocalVar%VS_GenPwr + LocalVarOutData(8) = LocalVar%VS_GenPwrF + LocalVarOutData(9) = LocalVar%GenSpeed + LocalVarOutData(10) = LocalVar%RotSpeed + LocalVarOutData(11) = LocalVar%NacHeading + LocalVarOutData(12) = LocalVar%NacVane + LocalVarOutData(13) = LocalVar%NacVaneF + LocalVarOutData(14) = LocalVar%HorWindV + LocalVarOutData(15) = LocalVar%HorWindV_F + LocalVarOutData(16) = LocalVar%rootMOOP(1) + LocalVarOutData(17) = LocalVar%rootMOOPF(1) + LocalVarOutData(18) = LocalVar%BlPitch(1) + LocalVarOutData(19) = LocalVar%BlPitchCMeas + LocalVarOutData(20) = LocalVar%Azimuth + LocalVarOutData(21) = LocalVar%OL_Azimuth + LocalVarOutData(22) = LocalVar%AzUnwrapped + LocalVarOutData(23) = LocalVar%AzError + LocalVarOutData(24) = LocalVar%GenTqAz + LocalVarOutData(25) = LocalVar%AzBuffer(1) + LocalVarOutData(26) = LocalVar%NumBl + LocalVarOutData(27) = LocalVar%FA_Acc + LocalVarOutData(28) = LocalVar%NacIMU_FA_Acc + LocalVarOutData(29) = LocalVar%FA_AccHPF + LocalVarOutData(30) = LocalVar%FA_AccHPFI + LocalVarOutData(31) = LocalVar%FA_PitCom(1) + LocalVarOutData(32) = LocalVar%VS_RefSpd + LocalVarOutData(33) = LocalVar%VS_RefSpd_TSR + LocalVarOutData(34) = LocalVar%VS_RefSpd_TRA + LocalVarOutData(35) = LocalVar%VS_RefSpd_RL + LocalVarOutData(36) = LocalVar%PC_RefSpd + LocalVarOutData(37) = LocalVar%PC_RefSpd_SS + LocalVarOutData(38) = LocalVar%PC_RefSpd_PRC + LocalVarOutData(39) = LocalVar%RotSpeedF + LocalVarOutData(40) = LocalVar%GenSpeedF + LocalVarOutData(41) = LocalVar%GenTq + LocalVarOutData(42) = LocalVar%GenTqMeas + LocalVarOutData(43) = LocalVar%GenArTq + LocalVarOutData(44) = LocalVar%GenBrTq + LocalVarOutData(45) = LocalVar%IPC_PitComF(1) + LocalVarOutData(46) = LocalVar%PC_KP + LocalVarOutData(47) = LocalVar%PC_KI + LocalVarOutData(48) = LocalVar%PC_KD + LocalVarOutData(49) = LocalVar%PC_TF + LocalVarOutData(50) = LocalVar%PC_MaxPit + LocalVarOutData(51) = LocalVar%PC_MinPit + LocalVarOutData(52) = LocalVar%PC_PitComT + LocalVarOutData(53) = LocalVar%PC_PitComT_Last + LocalVarOutData(54) = LocalVar%PC_PitComTF + LocalVarOutData(55) = LocalVar%PC_PitComT_IPC(1) + LocalVarOutData(56) = LocalVar%PC_PwrErr + LocalVarOutData(57) = LocalVar%PC_SpdErr + LocalVarOutData(58) = LocalVar%IPC_AxisTilt_1P + LocalVarOutData(59) = LocalVar%IPC_AxisYaw_1P + LocalVarOutData(60) = LocalVar%IPC_AxisTilt_2P + LocalVarOutData(61) = LocalVar%IPC_AxisYaw_2P + LocalVarOutData(62) = LocalVar%axisTilt_1P + LocalVarOutData(63) = LocalVar%axisYaw_1P + LocalVarOutData(64) = LocalVar%axisYawF_1P + LocalVarOutData(65) = LocalVar%axisTilt_2P + LocalVarOutData(66) = LocalVar%axisYaw_2P + LocalVarOutData(67) = LocalVar%axisYawF_2P + LocalVarOutData(68) = LocalVar%IPC_KI(1) + LocalVarOutData(69) = LocalVar%IPC_KP(1) + LocalVarOutData(70) = LocalVar%IPC_IntSat + LocalVarOutData(71) = LocalVar%PC_State + LocalVarOutData(72) = LocalVar%PitCom(1) + LocalVarOutData(73) = LocalVar%PitComAct(1) + LocalVarOutData(74) = LocalVar%SS_DelOmegaF + LocalVarOutData(75) = LocalVar%TestType + LocalVarOutData(76) = LocalVar%Kp_Float + LocalVarOutData(77) = LocalVar%VS_MaxTq + LocalVarOutData(78) = LocalVar%VS_LastGenTrq + LocalVarOutData(79) = LocalVar%VS_LastGenPwr + LocalVarOutData(80) = LocalVar%VS_MechGenPwr + LocalVarOutData(81) = LocalVar%VS_SpdErrAr + LocalVarOutData(82) = LocalVar%VS_SpdErrBr + LocalVarOutData(83) = LocalVar%VS_SpdErr + LocalVarOutData(84) = LocalVar%VS_State + LocalVarOutData(85) = LocalVar%VS_Rgn3Pitch + LocalVarOutData(86) = LocalVar%WE_Vw + LocalVarOutData(87) = LocalVar%WE_Vw_F + LocalVarOutData(88) = LocalVar%WE_VwI + LocalVarOutData(89) = LocalVar%WE_VwIdot + LocalVarOutData(90) = LocalVar%WE_Op + LocalVarOutData(91) = LocalVar%WE_Op_Last + LocalVarOutData(92) = LocalVar%VS_LastGenTrqF + LocalVarOutData(93) = LocalVar%PRC_WSE_F + LocalVarOutData(94) = LocalVar%PRC_R_Speed + LocalVarOutData(95) = LocalVar%PRC_R_Torque + LocalVarOutData(96) = LocalVar%PRC_R_Pitch + LocalVarOutData(97) = LocalVar%PRC_R_Total + LocalVarOutData(98) = LocalVar%PRC_Min_Pitch + LocalVarOutData(99) = LocalVar%PS_Min_Pitch + LocalVarOutData(100) = LocalVar%OL_Index + LocalVarOutData(101) = LocalVar%Fl_PitCom + LocalVarOutData(102) = LocalVar%NACIMU_FA_AccF + LocalVarOutData(103) = LocalVar%FA_AccF + LocalVarOutData(104) = LocalVar%FA_Hist + LocalVarOutData(105) = LocalVar%TRA_LastRefSpd + LocalVarOutData(106) = LocalVar%VS_RefSpeed + LocalVarOutData(107) = LocalVar%PtfmTDX + LocalVarOutData(108) = LocalVar%PtfmTDY + LocalVarOutData(109) = LocalVar%PtfmTDZ + LocalVarOutData(110) = LocalVar%PtfmRDX + LocalVarOutData(111) = LocalVar%PtfmRDY + LocalVarOutData(112) = LocalVar%PtfmRDZ + LocalVarOutData(113) = LocalVar%PtfmTVX + LocalVarOutData(114) = LocalVar%PtfmTVY + LocalVarOutData(115) = LocalVar%PtfmTVZ + LocalVarOutData(116) = LocalVar%PtfmRVX + LocalVarOutData(117) = LocalVar%PtfmRVY + LocalVarOutData(118) = LocalVar%PtfmRVZ + LocalVarOutData(119) = LocalVar%PtfmTAX + LocalVarOutData(120) = LocalVar%PtfmTAY + LocalVarOutData(121) = LocalVar%PtfmTAZ + LocalVarOutData(122) = LocalVar%PtfmRAX + LocalVarOutData(123) = LocalVar%PtfmRAY + LocalVarOutData(124) = LocalVar%PtfmRAZ + LocalVarOutData(125) = LocalVar%CC_DesiredL(1) + LocalVarOutData(126) = LocalVar%CC_ActuatedL(1) + LocalVarOutData(127) = LocalVar%CC_ActuatedDL(1) + LocalVarOutData(128) = LocalVar%StC_Input(1) + LocalVarOutData(129) = LocalVar%Flp_Angle(1) + LocalVarOutData(130) = LocalVar%RootMyb_Last(1) + LocalVarOutData(131) = LocalVar%ACC_INFILE_SIZE + LocalVarOutData(132) = LocalVar%AWC_complexangle(1) + LocalVarOutData(133) = LocalVar%ZMQ_ID + LocalVarOutData(134) = LocalVar%ZMQ_YawOffset + LocalVarOutData(135) = LocalVar%ZMQ_TorqueOffset + LocalVarOutData(136) = LocalVar%ZMQ_PitOffset(1) + LocalVarOutData(137) = LocalVar%ZMQ_R_Speed + LocalVarOutData(138) = LocalVar%ZMQ_R_Torque + LocalVarOutData(139) = LocalVar%ZMQ_R_Pitch + LocalVarOutStrings = [CHARACTER(15) :: 'iStatus', 'RestartWSE', 'Time', 'DT', 'n_DT', & + 'Time_Last', 'VS_GenPwr', 'VS_GenPwrF', 'GenSpeed', 'RotSpeed', & + 'NacHeading', 'NacVane', 'NacVaneF', 'HorWindV', 'HorWindV_F', & + 'rootMOOP', 'rootMOOPF', 'BlPitch', 'BlPitchCMeas', 'Azimuth', & + 'OL_Azimuth', 'AzUnwrapped', 'AzError', 'GenTqAz', 'AzBuffer', & + 'NumBl', 'FA_Acc', 'NacIMU_FA_Acc', 'FA_AccHPF', 'FA_AccHPFI', & + 'FA_PitCom', 'VS_RefSpd', 'VS_RefSpd_TSR', 'VS_RefSpd_TRA', 'VS_RefSpd_RL', & + 'PC_RefSpd', 'PC_RefSpd_SS', 'PC_RefSpd_PRC', 'RotSpeedF', 'GenSpeedF', & + 'GenTq', 'GenTqMeas', 'GenArTq', 'GenBrTq', 'IPC_PitComF', & + 'PC_KP', 'PC_KI', 'PC_KD', 'PC_TF', 'PC_MaxPit', & + 'PC_MinPit', 'PC_PitComT', 'PC_PitComT_Last', 'PC_PitComTF', 'PC_PitComT_IPC', & + 'PC_PwrErr', 'PC_SpdErr', 'IPC_AxisTilt_1P', 'IPC_AxisYaw_1P', 'IPC_AxisTilt_2P', & + 'IPC_AxisYaw_2P', 'axisTilt_1P', 'axisYaw_1P', 'axisYawF_1P', 'axisTilt_2P', & + 'axisYaw_2P', 'axisYawF_2P', 'IPC_KI', 'IPC_KP', 'IPC_IntSat', & + 'PC_State', 'PitCom', 'PitComAct', 'SS_DelOmegaF', 'TestType', & + 'Kp_Float', 'VS_MaxTq', 'VS_LastGenTrq', 'VS_LastGenPwr', 'VS_MechGenPwr', & + 'VS_SpdErrAr', 'VS_SpdErrBr', 'VS_SpdErr', 'VS_State', 'VS_Rgn3Pitch', & + 'WE_Vw', 'WE_Vw_F', 'WE_VwI', 'WE_VwIdot', 'WE_Op', & + 'WE_Op_Last', 'VS_LastGenTrqF', 'PRC_WSE_F', 'PRC_R_Speed', 'PRC_R_Torque', & + 'PRC_R_Pitch', 'PRC_R_Total', 'PRC_Min_Pitch', 'PS_Min_Pitch', 'OL_Index', & + 'Fl_PitCom', 'NACIMU_FA_AccF', 'FA_AccF', 'FA_Hist', 'TRA_LastRefSpd', & + 'VS_RefSpeed', 'PtfmTDX', 'PtfmTDY', 'PtfmTDZ', 'PtfmRDX', & + 'PtfmRDY', 'PtfmRDZ', 'PtfmTVX', 'PtfmTVY', 'PtfmTVZ', & + 'PtfmRVX', 'PtfmRVY', 'PtfmRVZ', 'PtfmTAX', 'PtfmTAY', & + 'PtfmTAZ', 'PtfmRAX', 'PtfmRAY', 'PtfmRAZ', 'CC_DesiredL', & + 'CC_ActuatedL', 'CC_ActuatedDL', 'StC_Input', 'Flp_Angle', 'RootMyb_Last', & + 'ACC_INFILE_SIZE', 'AWC_complexangle', 'ZMQ_ID', 'ZMQ_YawOffset', 'ZMQ_TorqueOffset', & + 'ZMQ_PitOffset', 'ZMQ_R_Speed', 'ZMQ_R_Torque', 'ZMQ_R_Pitch'] ! Initialize debug file IF ((LocalVar%iStatus == 0) .OR. (LocalVar%iStatus == -9)) THEN ! .TRUE. if we're on the first call to the DLL IF (CntrPar%LoggingLevel > 0) THEN @@ -858,8 +906,8 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, ErrVar, avrSWAP, RootName, size_av CALL GetNewUnit(UnDb2, ErrVar) OPEN(unit=UnDb2, FILE=TRIM(RootName)//'.RO.dbg2') WRITE(UnDb2, *) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version) - WRITE(UnDb2, '(125(a20,TR5:))') 'Time', LocalVarOutStrings - WRITE(UnDb2, '(125(a20,TR5:))') + WRITE(UnDb2, '(140(a20,TR5:))') 'Time', LocalVarOutStrings + WRITE(UnDb2, '(140(a20,TR5:))') END IF IF (CntrPar%LoggingLevel > 2) THEN @@ -904,25 +952,25 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, ErrVar, avrSWAP, RootName, size_av ! Process DebugOutData, LocalVarOutData ! Remove very small numbers that cause ******** outputs DO I = 1,SIZE(DebugOutData) - IF (ABS(DebugOutData(I)) < 1D-99) THEN + IF (ABS(DebugOutData(I)) < 1E-99) THEN DebugOutData(I) = 0 END IF - IF (ABS(DebugOutData(I)) > 1D+99) THEN - DebugOutData(I) = 1D+99 + IF (ABS(DebugOutData(I)) > 1E+99) THEN + DebugOutData(I) = 1E+99 END IF END DO DO I = 1,SIZE(LocalVarOutData) - IF (ABS(LocalVarOutData(I)) < 1D-99) THEN + IF (ABS(LocalVarOutData(I)) < 1E-99) THEN LocalVarOutData(I) = 0 END IF - IF (ABS(LocalVarOutData(I)) > 1D+99) THEN - LocalVarOutData(I) = 1D+99 + IF (ABS(LocalVarOutData(I)) > 1E+99) THEN + LocalVarOutData(I) = 1E+99 END IF END DO ! Write debug files - FmtDat = "(F20.5,TR5,124(ES20.5E2,TR5:))" ! The format of the debugging data + FmtDat = "(F20.5,TR5,139(ES20.5E2,TR5:))" ! The format of the debugging data IF ( MOD(LocalVar%n_DT, CntrPar%n_DT_Out) == 0) THEN IF(CntrPar%LoggingLevel > 0) THEN WRITE (UnDb, TRIM(FmtDat)) LocalVar%Time, DebugOutData @@ -945,4 +993,4 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, ErrVar, avrSWAP, RootName, size_av ENDIF END SUBROUTINE Debug -END MODULE ROSCO_IO +END MODULE ROSCO_IO \ No newline at end of file diff --git a/rosco/controller/src/ROSCO_Types.f90 b/rosco/controller/src/ROSCO_Types.f90 index 64385192..a1c53f6b 100644 --- a/rosco/controller/src/ROSCO_Types.f90 +++ b/rosco/controller/src/ROSCO_Types.f90 @@ -1,5 +1,5 @@ ! ROSCO Registry -! This file is automatically generated by write_registry.py using ROSCO v2.9.0 +! This file is automatically generated by write_registry.py using ROSCO v2.9.4 ! For any modification to the registry, please edit the rosco_types.yaml accordingly MODULE ROSCO_Types @@ -83,10 +83,17 @@ MODULE ROSCO_Types REAL(DbKi) :: SS_VSGain ! Variable speed torque controller setpoint smoother gain, [-]. REAL(DbKi) :: SS_PCGain ! Collective pitch controller setpoint smoother gain, [-]. INTEGER(IntKi) :: PRC_Mode ! Power reference tracking mode, 0- use standard rotor speed set points, 1- use PRC rotor speed setpoints + INTEGER(IntKi) :: PRC_Comm ! Power reference communication mode, 0- use constant DISCON inputs, 1- use open loop inputs, 2- use ZMQ inputs REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PRC_WindSpeeds ! Array of wind speeds used in rotor speed vs. wind speed lookup table REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PRC_GenSpeeds ! Array of rotor speeds corresponding to PRC_WindSpeeds INTEGER(IntKi) :: PRC_n ! Number of elements in PRC_WindSpeeds and PRC_GenSpeeds array REAL(DbKi) :: PRC_LPF_Freq ! Frequency of the low pass filter on the wind speed estimate used to set PRC_GenSpeeds [rad/s] + REAL(DbKi) :: PRC_R_Torque ! Power rating through changing the rated torque, default is 1, effective above rated [-] + REAL(DbKi) :: PRC_R_Speed ! Power rating through changing the rated generator speed, default is 1, effective above rated [-] + REAL(DbKi) :: PRC_R_Pitch ! Power rating through changing the fine pitch angle, default is 1, effective below rated [-] + INTEGER(IntKi) :: PRC_Table_n ! Number of elements in PRC_R to _Pitch table + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PRC_Pitch_Table ! Table of fine pitch versus PRC_R_Table, length should be PRC_Table_n [rad] + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PRC_R_Table ! Table of turbine rating versus fine pitch (PRC_Pitch_Table), length should be PRC_Table_n, default is 1 [-] INTEGER(IntKi) :: WE_Mode ! Wind speed estimator mode {0 - One-second low pass filtered hub height wind speed, 1 - Imersion and Invariance Estimator (Ortega et al.) REAL(DbKi) :: WE_BladeRadius ! Blade length [m] INTEGER(IntKi) :: WE_CP_n ! Amount of parameters in the Cp array @@ -126,10 +133,15 @@ MODULE ROSCO_Types REAL(DbKi) :: Flp_MaxPit ! Maximum (and minimum) flap pitch angle [rad] CHARACTER(1024) :: OL_Filename ! Input file with open loop timeseries INTEGER(IntKi) :: OL_Mode ! Open loop control mode {0 - no open loop control, 1 - open loop control vs. time, 2 - open loop control vs. wind speed} + INTEGER(IntKi) :: OL_BP_Mode ! Open loop control mode {0 - no open loop control, 1 - open loop control vs. time, 2 - open loop control vs. wind speed} + REAL(DbKi) :: OL_BP_FiltFreq ! First order low pass filter cutoff frequency for open loop breakpoint INTEGER(IntKi) :: Ind_Breakpoint ! The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1) INTEGER(IntKi), DIMENSION(:), ALLOCATABLE :: Ind_BldPitch ! The columns in OL_Filename that contains the blade pitch inputs (1,2,3) in rad INTEGER(IntKi) :: Ind_GenTq ! The column in OL_Filename that contains the generator torque in Nm INTEGER(IntKi) :: Ind_YawRate ! The column in OL_Filename that contains the generator torque in Nm + INTEGER(IntKi) :: Ind_R_Speed ! The column in OL_Filename that contains the R_Speed input + INTEGER(IntKi) :: Ind_R_Torque ! The column in OL_Filename that contains the R_Torque input + INTEGER(IntKi) :: Ind_R_Pitch ! The column in OL_Filename that contains the R_Pitch input INTEGER(IntKi) :: Ind_Azimuth ! The column in OL_Filename that contains the desired azimuth position in rad (used if OL_Mode = 2) REAL(DbKi), DIMENSION(:), ALLOCATABLE :: RP_Gains ! PID gains and Tf on derivative term for rotor position control (used if OL_Mode = 2) INTEGER(IntKi), DIMENSION(:), ALLOCATABLE :: Ind_CableControl ! The column in OL_Filename that contains the cable control inputs in m @@ -143,6 +155,9 @@ MODULE ROSCO_Types REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_GenTq ! Open loop generator torque timeseries REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_YawRate ! Open loop yaw rate timeseries REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_Azimuth ! Open loop azimuth timeseries + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_R_Speed ! Open loop R_Speed timeseries + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_R_Torque ! Open loop R_Torque timeseries + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_R_Pitch ! Open loop R_Pitch timeseries REAL(DbKi), DIMENSION(:,:), ALLOCATABLE :: OL_Channels ! Open loop channels in timeseries INTEGER(IntKi) :: PA_Mode ! Pitch actuator mode {0 - not used, 1 - first order filter, 2 - second order filter} REAL(DbKi) :: PA_CornerFreq ! Pitch actuator bandwidth/cut-off frequency [rad/s] @@ -248,6 +263,7 @@ MODULE ROSCO_Types TYPE, PUBLIC :: LocalVariables INTEGER(IntKi) :: iStatus ! Initialization status + INTEGER(IntKi) :: RestartWSE ! Restart WSE flag, 0 - restart, 1- not, to mirror iStatus REAL(DbKi) :: Time ! Time [s] REAL(DbKi) :: DT ! Time step [s] LOGICAL :: WriteThisStep ! Write an output line this time step @@ -259,7 +275,9 @@ MODULE ROSCO_Types REAL(DbKi) :: RotSpeed ! Rotor speed (LSS) [rad/s] REAL(DbKi) :: NacHeading ! Nacelle heading of the turbine w.r.t. north [deg] REAL(DbKi) :: NacVane ! Nacelle vane angle [deg] + REAL(DbKi) :: NacVaneF ! Filtered nacelle vane angle [deg] REAL(DbKi) :: HorWindV ! Hub height wind speed m/s + REAL(DbKi) :: HorWindV_F ! Filtered hub height wind speed m/s REAL(DbKi) :: rootMOOP(3) ! Blade root bending moment [Nm] REAL(DbKi) :: rootMOOPF(3) ! Filtered Blade root bending moment [Nm] REAL(DbKi) :: BlPitch(3) ! Blade pitch [rad] @@ -296,7 +314,7 @@ MODULE ROSCO_Types REAL(DbKi) :: PC_TF ! First-order filter parameter for derivative action REAL(DbKi) :: PC_MaxPit ! Maximum pitch setting in pitch controller (variable) [rad]. REAL(DbKi) :: PC_MinPit ! Minimum pitch setting in pitch controller (variable) [rad]. - REAL(DbKi) :: PC_PitComT ! Collective pitch commmand from PI control [rad]. + REAL(DbKi) :: PC_PitComT ! Total command pitch based on the sum of the proportional and integral terms [rad]. REAL(DbKi) :: PC_PitComT_Last ! Last total command pitch based on the sum of the proportional and integral terms [rad]. REAL(DbKi) :: PC_PitComTF ! Filtered Total command pitch based on the sum of the proportional and integral terms [rad]. REAL(DbKi) :: PC_PitComT_IPC(3) ! Total command pitch based on the sum of the proportional and integral terms, including IPC term [rad]. @@ -334,8 +352,17 @@ MODULE ROSCO_Types REAL(DbKi) :: WE_Vw_F ! Filtered estimated wind speed [m/s] REAL(DbKi) :: WE_VwI ! Integrated wind speed quantity for estimation [m/s] REAL(DbKi) :: WE_VwIdot ! Differentiated integrated wind speed quantity for estimation [m/s] + INTEGER(IntKi) :: WE_Op ! WSE Operational state (0- not operating, 1-operating) + INTEGER(IntKi) :: WE_Op_Last ! Last WSE Operational state (0- not operating, 1-operating) REAL(DbKi) :: VS_LastGenTrqF ! Differentiated integrated wind speed quantity for estimation [m/s] REAL(DbKi) :: PRC_WSE_F ! Filtered wind speed estimate for power reference control + REAL(DbKi) :: PRC_R_Speed ! Instantaneous PRC_R_Speed + REAL(DbKi) :: PRC_R_Torque ! Instantaneous PRC_R_Torque + REAL(DbKi) :: PRC_R_Pitch ! Instantaneous PRC_R_Pitch + REAL(DbKi) :: PRC_R_Total ! Instantaneous PRC_R_Total + REAL(DbKi) :: PRC_Min_Pitch ! Instantaneous PRC_Min_Pitch + REAL(DbKi) :: PS_Min_Pitch ! Instantaneous peak shaving + REAL(DbKi) :: OL_Index ! Open loop indexing variable (time or wind speed) LOGICAL :: SD ! Shutdown, .FALSE. if inactive, .TRUE. if active REAL(DbKi) :: Fl_PitCom ! Shutdown, .FALSE. if inactive, .TRUE. if active REAL(DbKi) :: NACIMU_FA_AccF ! None @@ -375,6 +402,9 @@ MODULE ROSCO_Types REAL(DbKi) :: ZMQ_YawOffset ! Yaw offset command, [rad] REAL(DbKi) :: ZMQ_TorqueOffset ! Torque offset command, [Nm] REAL(DbKi) :: ZMQ_PitOffset(3) ! Pitch command offset provided by ZeroMQ + REAL(DbKi) :: ZMQ_R_Speed ! R_Speed command provided by ZeroMQ + REAL(DbKi) :: ZMQ_R_Torque ! R_Torque command provided by ZeroMQ + REAL(DbKi) :: ZMQ_R_Pitch ! R_Pitch command provided by ZeroMQ TYPE(WE) :: WE ! Wind speed estimator parameters derived type TYPE(FilterParameters) :: FP ! Filter parameters derived type TYPE(piParams) :: piP ! PI parameters derived type diff --git a/rosco/controller/src/ReadSetParameters.f90 b/rosco/controller/src/ReadSetParameters.f90 index b76ef5f5..a1dbdfde 100644 --- a/rosco/controller/src/ReadSetParameters.f90 +++ b/rosco/controller/src/ReadSetParameters.f90 @@ -17,6 +17,7 @@ MODULE ReadSetParameters USE Constants USE Functions + USE Filters USE SysSubs USE ROSCO_Helpers IMPLICIT NONE @@ -217,6 +218,8 @@ SUBROUTINE SetParameters(avrSWAP, accINFILE, size_avcMSG, CntrPar, LocalVar, obj ! Wind speed estimator initialization LocalVar%WE_Vw = LocalVar%HorWindV LocalVar%WE_VwI = LocalVar%WE_Vw - CntrPar%WE_Gamma*LocalVar%RotSpeed + LocalVar%WE_Op = 1 + LocalVar%WE_Op_Last = 1 ! Setpoint Smoother initialization to zero LocalVar%SS_DelOmegaF = 0 @@ -251,6 +254,22 @@ SUBROUTINE SetParameters(avrSWAP, accINFILE, size_avcMSG, CntrPar, LocalVar, obj ENDIF + + ! Initialize other values on each timestep + + ! Open Loop index + IF (CntrPar%OL_BP_Mode == 0) THEN + LocalVar%OL_Index = LocalVar%Time + + ELSE + ! Wind speed, OL_BP_Mode = 1 + LocalVar%OL_Index = LocalVar%WE_Vw + IF (CntrPar%OL_BP_FiltFreq > 0) THEN + LocalVar%OL_Index = LPFilter(LocalVar%WE_Vw, LocalVar%DT,CntrPar%OL_BP_FiltFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) + ENDIF + + ENDIF + END SUBROUTINE SetParameters ! ----------------------------------------------------------------------------------- @@ -440,6 +459,13 @@ SUBROUTINE ReadControlParameterFileSub(CntrPar, LocalVar, accINFILE, accINFILE_s IF (ErrVar%aviFAIL < 0) RETURN !------------ POWER REFERENCE TRACKING SETPOINTS -------------- + CALL ParseInput(FileLines, 'PRC_Comm', CntrPar%PRC_Comm, accINFILE(1), ErrVar, CntrPar%PRC_Mode .NE. 1, UnEc) + CALL ParseInput(FileLines, 'PRC_R_Torque', CntrPar%PRC_R_Torque, accINFILE(1), ErrVar, (CntrPar%PRC_Mode .NE. 2) .OR. (CntrPar%PRC_Comm .NE. PRC_Comm_Constant), UnEc) + CALL ParseInput(FileLines, 'PRC_R_Speed', CntrPar%PRC_R_Speed, accINFILE(1), ErrVar, (CntrPar%PRC_Mode .NE. 2) .OR. (CntrPar%PRC_Comm .NE. PRC_Comm_Constant), UnEc) + CALL ParseInput(FileLines, 'PRC_R_Pitch', CntrPar%PRC_R_Pitch, accINFILE(1), ErrVar, (CntrPar%PRC_Mode .NE. 2) .OR. (CntrPar%PRC_Comm .NE. PRC_Comm_Constant), UnEc) + CALL ParseInput(FileLines, 'PRC_Table_n', CntrPar%PRC_Table_n, accINFILE(1), ErrVar, (CntrPar%PRC_Mode .NE. 2) .OR. (CntrPar%PRC_R_Pitch == 1.0), UnEc) + CALL ParseAry(FileLines, 'PRC_R_Table', CntrPar%PRC_R_Table, CntrPar%PRC_Table_n, accINFILE(1), ErrVar, (CntrPar%PRC_Mode .NE. 2) .OR. (CntrPar%PRC_R_Pitch == 1.0), UnEc) + CALL ParseAry(FileLines, 'PRC_Pitch_Table', CntrPar%PRC_Pitch_Table, CntrPar%PRC_Table_n, accINFILE(1), ErrVar, (CntrPar%PRC_Mode .NE. 2) .OR. (CntrPar%PRC_R_Pitch == 1.0), UnEc) CALL ParseInput(FileLines, 'PRC_n', CntrPar%PRC_n, accINFILE(1), ErrVar, CntrPar%PRC_Mode == 0) CALL ParseInput(FileLines, 'PRC_LPF_Freq', CntrPar%PRC_LPF_Freq, accINFILE(1), ErrVar, CntrPar%PRC_Mode == 0) CALL ParseAry( FileLines, 'PRC_WindSpeeds', CntrPar%PRC_WindSpeeds, CntrPar%PRC_n, accINFILE(1), ErrVar, CntrPar%PRC_Mode == 0) @@ -509,11 +535,16 @@ SUBROUTINE ReadControlParameterFileSub(CntrPar, LocalVar, accINFILE, accINFILE_s !------------ Open loop input ------------ ! Indices can be left 0 by default, checked later CALL ParseInput(FileLines, 'OL_Filename', CntrPar%OL_Filename, accINFILE(1), ErrVar, CntrPar%OL_Mode == 0, UnEc) + CALL ParseInput(FileLines, 'OL_BP_Mode', CntrPar%OL_BP_Mode, accINFILE(1), ErrVar, CntrPar%OL_Mode == 0, UnEc) + CALL ParseInput(FileLines, 'OL_BP_FiltFreq', CntrPar%OL_BP_FiltFreq, accINFILE(1), ErrVar, CntrPar%OL_Mode == 0, UnEc) CALL ParseInput(FileLines, 'Ind_Breakpoint', CntrPar%Ind_Breakpoint, accINFILE(1), ErrVar, UnEc=UnEc) CALL ParseAry( FileLines, 'Ind_BldPitch', CntrPar%Ind_BldPitch, 3, accINFILE(1), ErrVar, UnEc=UnEc) CALL ParseInput(FileLines, 'Ind_GenTq', CntrPar%Ind_GenTq, accINFILE(1), ErrVar, UnEc=UnEc) CALL ParseInput(FileLines, 'Ind_YawRate', CntrPar%Ind_YawRate, accINFILE(1), ErrVar, UnEc=UnEc) CALL ParseInput(FileLines, 'Ind_Azimuth', CntrPar%Ind_Azimuth, accINFILE(1), ErrVar, CntrPar%OL_Mode .NE. 2, UnEc=UnEc) + CALL ParseInput(FileLines, 'Ind_R_Speed', CntrPar%Ind_R_Speed, accINFILE(1), ErrVar, CntrPar%OL_Mode .NE. 2, UnEc=UnEc) + CALL ParseInput(FileLines, 'Ind_R_Torque', CntrPar%Ind_R_Torque, accINFILE(1), ErrVar, CntrPar%OL_Mode .NE. 2, UnEc=UnEc) + CALL ParseInput(FileLines, 'Ind_R_Pitch', CntrPar%Ind_R_Pitch, accINFILE(1), ErrVar, CntrPar%OL_Mode .NE. 2, UnEc=UnEc) CALL ParseAry( FileLines, 'RP_Gains', CntrPar%RP_Gains, 4, accINFILE(1), ErrVar, CntrPar%OL_Mode .NE. 2, UnEc=UnEc) IF (ErrVar%aviFAIL < 0) RETURN @@ -635,6 +666,27 @@ SUBROUTINE ReadControlParameterFileSub(CntrPar, LocalVar, accINFILE, accINFILE_s END IF ENDIF + IF (CntrPar%Ind_R_Speed > 0) THEN + IF (CntrPar%OL_Mode == 1) THEN + OL_String = TRIM(OL_String)//' R_Speed ' + OL_Count = OL_Count + 1 + END IF + ENDIF + + IF (CntrPar%Ind_R_Torque > 0) THEN + IF (CntrPar%OL_Mode == 1) THEN + OL_String = TRIM(OL_String)//' R_Torque ' + OL_Count = OL_Count + 1 + END IF + ENDIF + + IF (CntrPar%Ind_R_Pitch > 0) THEN + IF (CntrPar%OL_Mode == 1) THEN + OL_String = TRIM(OL_String)//' R_Pitch ' + OL_Count = OL_Count + 1 + END IF + ENDIF + N_OL_Cables = 0 IF (ANY(CntrPar%Ind_CableControl > 0)) THEN DO I = 1,SIZE(CntrPar%Ind_CableControl) @@ -695,6 +747,18 @@ SUBROUTINE ReadControlParameterFileSub(CntrPar, LocalVar, accINFILE, accINFILE_s IF (CntrPar%Ind_Azimuth > 0) THEN CntrPar%OL_Azimuth = Unwrap(CntrPar%OL_Channels(:,CntrPar%Ind_Azimuth),ErrVar) ENDIF + + IF (CntrPar%Ind_R_Speed > 0) THEN + CntrPar%OL_R_Speed = CntrPar%OL_Channels(:,CntrPar%Ind_R_Speed) + ENDIF + + IF (CntrPar%Ind_R_Torque > 0) THEN + CntrPar%OL_R_Torque = CntrPar%OL_Channels(:,CntrPar%Ind_R_Torque) + ENDIF + + IF (CntrPar%Ind_R_Pitch > 0) THEN + CntrPar%OL_R_Pitch = CntrPar%OL_Channels(:,CntrPar%Ind_R_Pitch) + ENDIF IF (ANY(CntrPar%Ind_CableControl > 0)) THEN ALLOCATE(CntrPar%OL_CableControl(N_OL_Cables,SIZE(CntrPar%OL_Channels,DIM=1))) @@ -753,8 +817,8 @@ SUBROUTINE ReadCpFile(CntrPar,PerfData, ErrVar) CurLine = 1 CALL GetNewUnit(UnPerfParameters, ErrVar) - OPEN(unit=UnPerfParameters, file=TRIM(CntrPar%PerfFileName), status='old', action='read') ! Should put input file into DISCON.IN - + OPEN(unit=UnPerfParameters, file=TRIM(CntrPar%PerfFileName), status='old', action='read') + ! ----------------------- Axis Definitions ------------------------ CALL ReadEmptyLine(UnPerfParameters,CurLine) CALL ReadEmptyLine(UnPerfParameters,CurLine) @@ -1173,8 +1237,42 @@ SUBROUTINE CheckInputs(LocalVar, CntrPar, avrSWAP, ErrVar, size_avcMSG) ErrVar%ErrMsg = 'SS_PCGain must be greater than zero.' ENDIF - IF (CntrPar%PRC_Mode > 0) THEN - PRINT *, "Note: PRC Mode = ", CntrPar%PRC_Mode, ", which will ignore VS_RefSpeed, VS_TSRopt, and PC_RefSpeed" + IF ((CntrPar%PRC_Mode < 0) .OR. (CntrPar%PRC_Mode > 2)) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'PRC_Mode must be 0, 1, or 2.' + ENDIF + + IF (CntrPar%PRC_Mode == 2) THEN + PRINT *, "Note: PRC Mode = ", CntrPar%PRC_Mode, ", which will affect VS_RefSpeed, VS_TSRopt, and PC_RefSpeed" + + IF (CntrPar%PRC_Comm == 0) THEN + IF (CntrPar%PRC_R_Pitch < 0) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'PRC_R_Pitch must be greater than or equal to zero.' + ENDIF + + IF (CntrPar%PRC_R_Speed < 0) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'PRC_R_Speed must be greater than or equal to zero.' + ENDIF + + IF (CntrPar%PRC_R_Torque < 0) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'PRC_R_Torque must be greater than or equal to zero.' + ENDIF + + ENDIF + + IF ((CntrPar%PRC_Comm == 1) .AND. (CntrPar%OL_Mode .NE. 1)) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'OL_Mode must be 1 to use open loop inputs for power control (PRC_Comm = 1).' + ENDIF + + IF ((CntrPar%PRC_Comm == 2) .AND. (CntrPar%ZMQ_Mode .NE. 1)) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'ZMQ_Mode must be 1 to use ZeroMQ inputs for power control (PRC_Comm = 2).' + ENDIF + ENDIF !------- WIND SPEED ESTIMATOR --------------------------------------------- @@ -1305,7 +1403,10 @@ SUBROUTINE CheckInputs(LocalVar, CntrPar, avrSWAP, ErrVar, size_avcMSG) ALLOCATE(All_OL_Indices(5)) ! Will need to increase to 5 when IPC All_OL_Indices = (/CntrPar%Ind_BldPitch, & CntrPar%Ind_GenTq, & - CntrPar%Ind_YawRate/) + CntrPar%Ind_YawRate, & + CntrPar%Ind_R_Speed, & + CntrPar%Ind_R_Torque, & + CntrPar%Ind_R_Pitch/) DO I = 1,SIZE(CntrPar%Ind_CableControl) Call AddToList(All_OL_Indices, CntrPar%Ind_CableControl(I)) @@ -1351,6 +1452,20 @@ SUBROUTINE CheckInputs(LocalVar, CntrPar, avrSWAP, ErrVar, size_avcMSG) ErrVar%ErrMsg = 'CC_Mode must be 2 if using open loop struct control via Ind_StructControl' ENDIF + IF ((CntrPar%OL_BP_Mode < 0) .OR. (CntrPar%OL_BP_Mode > 1)) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'OL_BP_Mode must be 0 or 1.' + ENDIF + + IF (CntrPar%OL_BP_FiltFreq < 0) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'OL_BP_FiltFreq must be greater than or equal to 0.' + ENDIF + + IF ((CntrPar%OL_BP_Mode == 1) .AND. (CntrPar%OL_Mode == 2)) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'Rotor position control (OL_Mode = 2) is not compatible with wind speed breakpoints (OL_BP_Mode = 1)' + ENDIF ENDIF diff --git a/rosco/controller/src/ZeroMQInterface.f90 b/rosco/controller/src/ZeroMQInterface.f90 index 9a3c1d19..e08aa7e1 100644 --- a/rosco/controller/src/ZeroMQInterface.f90 +++ b/rosco/controller/src/ZeroMQInterface.f90 @@ -12,7 +12,7 @@ SUBROUTINE UpdateZeroMQ(LocalVar, CntrPar, ErrVar) TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar character(256) :: zmq_address - real(C_DOUBLE) :: setpoints(5) + real(C_DOUBLE) :: setpoints(8) real(C_DOUBLE) :: turbine_measurements(17) CHARACTER(*), PARAMETER :: RoutineName = 'UpdateZeroMQ' @@ -24,7 +24,7 @@ subroutine zmq_client(zmq_address, measurements, setpoints) bind(C, name='zmq_cl implicit none character(C_CHAR), intent(out) :: zmq_address(*) real(C_DOUBLE) :: measurements(17) - real(C_DOUBLE) :: setpoints(5) + real(C_DOUBLE) :: setpoints(8) end subroutine zmq_client end interface #endif @@ -73,6 +73,9 @@ end subroutine zmq_client LocalVar%ZMQ_PitOffset(1) = setpoints(3) LocalVar%ZMQ_PitOffset(2) = setpoints(4) LocalVar%ZMQ_PitOffset(3) = setpoints(5) + LocalVar%ZMQ_R_Speed = setpoints(6) + LocalVar%ZMQ_R_Torque = setpoints(7) + LocalVar%ZMQ_R_Pitch = setpoints(8) ENDIF diff --git a/rosco/controller/src/zmq_client.c b/rosco/controller/src/zmq_client.c index d0aed4a5..76b76287 100644 --- a/rosco/controller/src/zmq_client.c +++ b/rosco/controller/src/zmq_client.c @@ -5,14 +5,16 @@ #include -void delete_blank_spaces_in_string(char *s) { - +void delete_blank_spaces_in_string(char *s) +{ int i,k=0; - for(i=0;s[i];i++) { + for(i=0;s[i];i++) + { s[i]=s[i+k]; - if(s[i]==" "|| s[i]=="\t") { - k++; - i--; + if(s[i]==" "|| s[i]=="\t") + { + k++; + i--; } } } @@ -21,8 +23,9 @@ void delete_blank_spaces_in_string(char *s) { int zmq_client ( char *zmq_address, double measurements[17], - double setpoints[5] -) { + double setpoints[8] +) +{ int num_measurements = 17; // Number of setpoints and measurements, respectively, and float precision (character length) int char_buffer_size_single = 20; // Char buffer for a single measurement int char_buffer_size_array = (num_measurements * (char_buffer_size_single + 1)); // Char buffer for full messages to and from ROSCO diff --git a/rosco/test/test_examples.py b/rosco/test/test_examples.py index 2aa44cc0..38539bdf 100644 --- a/rosco/test/test_examples.py +++ b/rosco/test/test_examples.py @@ -30,8 +30,9 @@ '24_floating_feedback', '25_rotor_position_control', '26_marine_hydro', - '27_power_ref_control', + '27_soft_cut_out', '28_tower_resonance', + '29_power_control', 'update_rosco_discons', ] diff --git a/rosco/toolbox/control_interface.py b/rosco/toolbox/control_interface.py index 488f438c..96c942b3 100644 --- a/rosco/toolbox/control_interface.py +++ b/rosco/toolbox/control_interface.py @@ -428,8 +428,8 @@ def _get_setpoints(self, id, measurements): setpoints = self.wfc_controller.update_setpoints(id, current_time, measurements) logger.info(f"Received setpoints {setpoints} from wfc_controller for time = {current_time} and id = {id}") - for s in self.wfc_interface["setpoints"]: - self.connections.setpoints[id][s] = setpoints.get(s, 0) + for i, s in enumerate(self.wfc_interface["setpoints"]): + self.connections.setpoints[id][s] = setpoints.get(s, self.wfc_interface['setpoints_default'][i]) logger.debug(f'Set setpoint {s} in the connections list to {setpoints.get(s,0)} for id = {id}') def wfc_controller(self, id, current_time, measurements): diff --git a/rosco/toolbox/controller.py b/rosco/toolbox/controller.py index bce35b2e..1fdec790 100644 --- a/rosco/toolbox/controller.py +++ b/rosco/toolbox/controller.py @@ -146,17 +146,23 @@ def __init__(self, controller_params): self.OL_Mode = controller_params['OL_Mode'] self.OL_Filename = controller_params['open_loop']['filename'] self.OL_Ind_Breakpoint = self.OL_Ind_GenTq = self.OL_Ind_YawRate = self.OL_Ind_Azimuth = 0 - self.OL_Ind_BldPitch = [0,0,0] - self.OL_Ind_CableControl = [0] - self.OL_Ind_StructControl = [0] + self.OL_Ind_R_Speed = self.OL_Ind_R_Torque = self.OL_Ind_R_Pitch = 0 + self.OL_Ind_BldPitch = [0,0,0] + self.OL_Ind_CableControl = [0] + self.OL_Ind_StructControl = [0] + self.OL_BP_Mode = 0 if self.OL_Mode: ol_params = controller_params['open_loop'] + self.OL_BP_Mode = ol_params['OL_BP_Mode'] self.OL_Ind_Breakpoint = ol_params['OL_Ind_Breakpoint'] self.OL_Ind_BldPitch = ol_params['OL_Ind_BldPitch'] self.OL_Ind_GenTq = ol_params['OL_Ind_GenTq'] self.OL_Ind_YawRate = ol_params['OL_Ind_YawRate'] self.OL_Ind_Azimuth = ol_params['OL_Ind_Azimuth'] + self.OL_Ind_R_Speed = ol_params['OL_Ind_R_Speed'] + self.OL_Ind_R_Torque = ol_params['OL_Ind_R_Torque'] + self.OL_Ind_R_Pitch = ol_params['OL_Ind_R_Pitch'] self.OL_Ind_CableControl = ol_params['OL_Ind_CableControl'] self.OL_Ind_StructControl = ol_params['OL_Ind_StructControl'] @@ -387,6 +393,23 @@ def tune_controller(self, turbine): self.ps.peak_shaving(self, turbine) self.ps.min_pitch_saturation(self,turbine) + # --- Power control --- + + PRC_Table_n = self.controller_params['DISCON']['PRC_Table_n'] + pitch_range = np.linspace(self.min_pitch,self.max_pitch, num=100) # radians + + # Cp at optimal TSR + Cp_TSR_opt = turbine.Cp.interp_surface(pitch_range,turbine.Cp.TSR_opt) + + # Solve for pitch that decreases Cp by R_range using linear interpolation + R_range = np.linspace(0,1,num=PRC_Table_n) + Cp_R = R_range * turbine.Cp.max + pitch_R = np.interp(Cp_R,np.flip(Cp_TSR_opt),np.flip(pitch_range)) # need to flip because interp wants xp to be increasing + + # Save values back to DISCON dict + self.controller_params['DISCON']['PRC_R_Table'] = R_range + self.controller_params['DISCON']['PRC_Pitch_Table'] = pitch_R + # --- Floating feedback term --- if self.Fl_Mode >= 1: # Floating feedback @@ -482,6 +505,7 @@ def tune_controller(self, turbine): if 'f_lpf_cornerfreq' in self.controller_params['filter_params']: self.f_lpf_cornerfreq = self.controller_params['filter_params']['f_lpf_cornerfreq'] + def tune_flap_controller(self,turbine): ''' Tune controller for distributed aerodynamic control @@ -727,8 +751,25 @@ class OpenLoopControl(object): ''' def __init__(self, **kwargs): - self.dt = 0.05 - self.t_max = 200 + self.dt = 0.05 # sec + self.t_max = 200 # sec + self.breakpoint = 'time' # or wind_speed + self.du = 0.5 # m/s + self.u_min = 5 # m/s + self.u_max = 30 # m/s + + self.allowed_controls = [ + 'blade_pitch', + 'generator_torque', + 'nacelle_yaw', + 'nacelle_yaw_rate', + 'cable_control', + 'struct_control', + 'R_speed', + 'R_torque', + 'R_pitch', + ] + self.allowed_breakpoints = ['time','wind_speed'] # Optional population class attributes from key word arguments for (k, w) in kwargs.items(): @@ -737,17 +778,20 @@ def __init__(self, **kwargs): except: pass - self.ol_timeseries = {} - self.ol_timeseries['time'] = np.arange(0,self.t_max,self.dt) - - self.allowed_controls = ['blade_pitch','generator_torque','nacelle_yaw','nacelle_yaw_rate','cable_control','struct_control'] + self.ol_series = {} + if self.breakpoint == 'time': + self.ol_series[self.breakpoint] = np.arange(0,self.t_max+self.dt,self.dt) + elif self.breakpoint == 'wind_speed': + self.ol_series[self.breakpoint] = np.arange(self.u_min,self.u_max+self.du,self.du) + else: + raise Exception(f'Breakpoint of {self.breakpoint} is not allowed. Available options are {self.allowed_breakpoints}.') def const_timeseries(self,control,value): - self.ol_timeseries[control] = value * np.ones(len(self.ol_timeseries['time'])) + self.ol_series[control] = value * np.ones(len(self.ol_series[self.breakpoint])) - def interp_timeseries(self,control,breakpoints,values,method='sigma'): + def interp_series(self,control,breakpoints,values,method='sigma'): # Error checking if not list_check(breakpoints) or len(breakpoints) == 1: @@ -762,58 +806,67 @@ def interp_timeseries(self,control,breakpoints,values,method='sigma'): # Check if control in allowed controls, cable_control_* is in cable_control if not any([ac in control for ac in self.allowed_controls]): raise Exception(f'Open loop control of {control} is not allowed') + + # Check that breakpoints are valid + if self.breakpoint == 'wind_speed': + if max(breakpoints) > self.u_max: + raise Exception(f'The maximum desired wind speed breakpoint ({max(breakpoints)}) is greater than u_max ({self.u_max})') + elif self.breakpoint == 'time': + if max(breakpoints) > self.t_max: + raise Exception(f'The maximum desired time breakpoint ({max(breakpoints)}) is greater than t_max ({self.t_max})') + + # Finally interpolate + if method == 'sigma': + self.ol_series[control] = multi_sigma(self.ol_series[self.breakpoint],breakpoints,values) + + elif method == 'linear': + self.ol_series[control] = np.interp(self.ol_series[self.breakpoint],breakpoints,values,left=values[0],right=values[-1]) - else: - # Finally interpolate - if method == 'sigma': - self.ol_timeseries[control] = multi_sigma(self.ol_timeseries['time'],breakpoints,values) - - elif method == 'linear': - interp_fcn = interpolate.interp1d(breakpoints,values,fill_value=values[-1],bounds_error=False) - self.ol_timeseries[control] = interp_fcn(self.ol_timeseries['time']) - - elif method == 'cubic': - interp_fcn = interpolate.interp1d(breakpoints,values,kind='cubic',fill_value=values[-1],bounds_error=False) - self.ol_timeseries[control] = interp_fcn(self.ol_timeseries['time']) + elif method == 'cubic': + interp_fcn = interpolate.Akima1DInterpolator(breakpoints,values,method='akima',extrapolate=True) #,kind='cubic',fill_value=values[-1],bounds_error=False) + self.ol_series[control] = interp_fcn(self.ol_series[self.breakpoint]) - else: - raise Exception(f'Open loop interpolation method {method} not supported') + else: + raise Exception(f'Open loop interpolation method {method} not supported') if control == 'nacelle_yaw': self.compute_yaw_rate() - def sine_timeseries(self,control,amplitude,period): + def sine_timeseries(self,control,amplitude,period,offset=0): if period <= 0: raise Exception('Open loop sine input period is <= 0') + + if self.breakpoint != 'time': + raise Exception(f'Only time breakpoints are allowed for sine timeseries') if control not in self.allowed_controls: raise Exception(f'Open loop control of {control} is not allowed') else: - self.ol_timeseries[control] = amplitude * np.sin(2 * np.pi * self.ol_timeseries['time'] / period) + self.ol_series[control] = amplitude * np.sin(2 * np.pi * self.ol_series['time'] / period) + offset if control == 'nacelle_yaw': self.compute_yaw_rate() def compute_yaw_rate(self): - self.ol_timeseries['nacelle_yaw_rate'] = np.concatenate(([0],np.diff(self.ol_timeseries['nacelle_yaw'])))/self.dt + self.ol_series['nacelle_yaw_rate'] = np.concatenate(([0],np.diff(self.ol_series['nacelle_yaw'])))/self.dt - def plot_timeseries(self): + def plot_series(self): ''' Debugging script for showing open loop timeseries ''' import matplotlib.pyplot as plt - fig, ax = plt.subplots(len(self.ol_timeseries)-1,1) + fig, ax = plt.subplots(len(self.ol_series)-1,1) i_ax = -1 - for ol_input in self.ol_timeseries: - if ol_input != 'time': + for ol_input in self.ol_series: + if ol_input not in ['time','wind_speed']: i_ax += 1 - if len(self.ol_timeseries)-1 == 1: - ax.plot(self.ol_timeseries['time'],self.ol_timeseries[ol_input]) + if len(self.ol_series)-1 == 1: + ax.plot(self.ol_series[self.breakpoint],self.ol_series[ol_input]) ax.set_ylabel(ol_input) else: - ax[i_ax].plot(self.ol_timeseries['time'],self.ol_timeseries[ol_input]) + ax[i_ax].plot(self.ol_series[self.breakpoint],self.ol_series[ol_input]) ax[i_ax].set_ylabel(ol_input) return fig, ax @@ -823,36 +876,43 @@ def write_input(self,ol_filename): Return open_loop dict for control params ''' - ol_timeseries = self.ol_timeseries + ol_series = self.ol_series # Init indices OL_Ind_Breakpoint = 1 - OL_Ind_Azimuth = OL_Ind_GenTq = OL_Ind_YawRate = 0 + OL_Ind_Azimuth = OL_Ind_GenTq = OL_Ind_YawRate = OL_Ind_R_Speed = OL_Ind_R_Torque = OL_Ind_R_Pitch = 0 OL_Ind_BldPitch = 3*[0] OL_Ind_CableControl = [] OL_Ind_StructControl = [] ol_index_counter = 0 # start input index at 2 - # Write time first, initialize OL matrix - if 'time' in ol_timeseries: - ol_control_array = ol_timeseries['time'] - Ind_Breakpoint = 1 + # Write breakpoint first, initialize OL matrix + if 'time' in ol_series: + ol_input_matrix = ol_series['time'] + elif 'wind_speed' in ol_series: + ol_input_matrix = ol_series['wind_speed'] else: - raise Exception('WARNING: no time index for open loop control. This is only index currently supported') + raise Exception('WARNING: no time or wind-speed index for open loop control. These are the only indices currently supported') + + if 'time' in ol_series and 'wind_speed' in ol_series: + raise Exception('WARNING: both time and wind_speed are in the OL input setup. Please check.') - for channel in ol_timeseries: + for channel in ol_series: # increment index counter first for 1-indexing in input file ol_index_counter += 1 - # skip writing for certain channels + # skip writing for certain channels, if already in ol_input_matrix skip_write = False # Set open loop index based on name if channel == 'time': OL_Ind_Breakpoint = ol_index_counter skip_write = True + elif channel == 'wind_speed': + OL_Ind_Breakpoint = ol_index_counter + skip_write = True elif channel == 'blade_pitch': # collective blade pitch OL_Ind_BldPitch = 3 * [ol_index_counter] elif channel == 'generator_torque': @@ -870,6 +930,12 @@ def write_input(self,ol_filename): OL_Ind_BldPitch[2] = ol_index_counter elif channel == 'azimuth': OL_Ind_Azimuth = ol_index_counter + elif channel == 'R_speed': + OL_Ind_R_Speed = ol_index_counter + elif channel == 'R_torque': + OL_Ind_R_Torque = ol_index_counter + elif channel == 'R_pitch': + OL_Ind_R_Pitch = ol_index_counter elif 'cable_control' in channel or 'struct_control' in channel: skip_write = True ol_index_counter -= 1 # don't increment counter @@ -878,32 +944,32 @@ def write_input(self,ol_filename): # append open loop input array for non-ptfm channels if not skip_write: - ol_control_array = np.c_[ol_control_array,ol_timeseries[channel]] + ol_input_matrix = np.c_[ol_input_matrix,ol_series[channel]] ol_index_counter += 1 # Increment counter so it's 1 more than time, just like above in each iteration # Cable control - is_cable_chan = np.array(['cable_control' in ol_chan for ol_chan in ol_timeseries.keys()]) + is_cable_chan = np.array(['cable_control' in ol_chan for ol_chan in ol_series.keys()]) if any(is_cable_chan): # if any channels are cable_control_* n_cable_chan = np.sum(is_cable_chan) - cable_chan_names = np.array(list(ol_timeseries.keys()))[is_cable_chan] + cable_chan_names = np.array(list(ol_series.keys()))[is_cable_chan] # Let's assume they are 1-indexed and all there, otherwise a key error will be thrown for cable_chan in cable_chan_names: - ol_control_array = np.c_[ol_control_array,ol_timeseries[cable_chan]] + ol_input_matrix = np.c_[ol_input_matrix,ol_series[cable_chan]] OL_Ind_CableControl.append(ol_index_counter) ol_index_counter += 1 # Struct control - is_struct_chan = ['struct_control' in ol_chan for ol_chan in ol_timeseries.keys()] + is_struct_chan = ['struct_control' in ol_chan for ol_chan in ol_series.keys()] if any(is_struct_chan): # if any channels are struct_control_* n_struct_chan = np.sum(np.array(is_struct_chan)) # Let's assume they are 1-indexed and all there, otherwise a key error will be thrown for i_chan in range(1,n_struct_chan+1): - ol_control_array = np.c_[ol_control_array,ol_timeseries[f'struct_control_{i_chan}']] + ol_input_matrix = np.c_[ol_input_matrix,ol_series[f'struct_control_{i_chan}']] OL_Ind_StructControl.append(ol_index_counter) ol_index_counter += 1 @@ -916,11 +982,20 @@ def write_input(self,ol_filename): # Write header headers = [''] * ol_index_counter units = [''] * ol_index_counter - header_line = '!\tTime' - unit_line = '!\t(sec.)' - headers[0] = 'Time' - units[0] = 'sec.' + if self.breakpoint == 'time': + header_line = '!\tTime' # TODO: not sure if we need header_line and headers, check struct control + unit_line = '!\t(sec.)' + + headers[0] = 'Time' + units[0] = 'sec.' + elif self.breakpoint == 'wind_speed': + header_line = '!\tWindSpeed' # TODO: not sure if we need header_line and headers, check struct control + unit_line = '!\t(m/s)' + + headers[0] = 'WindSpeed' + units[0] = 'm/s' + if OL_Ind_GenTq: headers[OL_Ind_GenTq-1] = 'GenTq' @@ -960,6 +1035,18 @@ def write_input(self,ol_filename): else: OL_Ind_StructControl = [0] + if OL_Ind_R_Speed: + headers[OL_Ind_R_Speed-1] = 'R_speed' + units[OL_Ind_R_Speed-1] = '(-)' + + if OL_Ind_R_Torque: + headers[OL_Ind_R_Torque-1] = 'R_Torque' + units[OL_Ind_R_Torque-1] = '(-)' + + if OL_Ind_R_Pitch: + headers[OL_Ind_R_Pitch-1] = 'R_Pitch' + units[OL_Ind_R_Pitch-1] = '(-)' + # Join headers and units header_line = '!' + '\t\t'.join(headers) + '\n' unit_line = '!' + '\t\t'.join(units) + '\n' @@ -968,7 +1055,7 @@ def write_input(self,ol_filename): f.write(unit_line) # Write lines - for ol_line in ol_control_array: + for ol_line in ol_input_matrix: line = ''.join(['{:<10.8f}\t'.format(val) for val in ol_line]) + '\n' f.write(line) @@ -982,6 +1069,14 @@ def write_input(self,ol_filename): open_loop['OL_Ind_Azimuth'] = OL_Ind_Azimuth open_loop['OL_Ind_CableControl'] = OL_Ind_CableControl open_loop['OL_Ind_StructControl'] = OL_Ind_StructControl + open_loop['OL_Ind_R_Speed'] = OL_Ind_R_Speed + open_loop['OL_Ind_R_Torque'] = OL_Ind_R_Torque + open_loop['OL_Ind_R_Pitch'] = OL_Ind_R_Pitch + if self.breakpoint == 'time': + open_loop['OL_BP_Mode'] = 0 + elif self.breakpoint == 'wind_speed': + open_loop['OL_BP_Mode'] = 1 + return open_loop diff --git a/rosco/toolbox/inputs/toolbox_schema.yaml b/rosco/toolbox/inputs/toolbox_schema.yaml index 44573ae1..3131f8bf 100644 --- a/rosco/toolbox/inputs/toolbox_schema.yaml +++ b/rosco/toolbox/inputs/toolbox_schema.yaml @@ -492,6 +492,12 @@ properties: description: Filename of open loop input that ROSCO reads type: string default: unused + OL_BP_Mode: + description: Breakpoint mode for open loop control, 0 - indexed by time (default), 1 - indexed by wind speed + type: number + default: 0 + minimum: 0 + maximum: 1 Ind_Breakpoint: description: Index (column, 1-indexed) of breakpoint (time) in open loop index type: number @@ -518,6 +524,21 @@ properties: type: number default: 0 description: The column in OL_Filename that contains the desired azimuth position in rad (used if OL_Mode = 2) + Ind_R_Speed: + description: Index (column, 1-indexed) of power rating via speed offset + type: number + default: 0 + minimum: 0 + Ind_R_Torque: + description: Index (column, 1-indexed) of power rating via torque offset + type: number + default: 0 + minimum: 0 + Ind_R_Pitch: + description: Index (column, 1-indexed) of power rating via pitch offset + type: number + default: 0 + minimum: 0 Ind_CableControl: type: array items: @@ -843,9 +864,41 @@ properties: type: number description: Frequency of the low pass filter on the wind speed estimate used to set PRC_GenSpeeds [rad/s] default: 0.078539 - PRC_n: + PRC_n: # figure out what to do with this! type: number description: Number of elements in PRC_WindSpeeds and PRC_GenSpeeds array + PRC_Comm: + type: number + description: Power reference communication mode when PRC_Mode = 2, 0- use constant DISCON inputs, 1- use open loop inputs, 2- use ZMQ inputs + default: 0 + PRC_R_Torque: + type: number + description: Constant power rating through changing the rated torque, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] + default: 1 + PRC_R_Speed: + type: number + description: Constant power rating through changing the rated generator speed, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective above rated [-] + default: 1 + PRC_R_Pitch: + type: number + description: Constant power rating through changing the fine pitch angle, used if PRC_Mode = 2, PRC_Comm = 0, default is 1, effective below rated [-] + default: 1 + PRC_Table_n: + type: number + description: Number of elements in PRC_R to _Pitch table. Used if PRC_Mode = 1. + default: 20 + PRC_Pitch_Table: + type: array + items: + type: number + description: Table of fine pitch versus PRC_R_Table, length should be PRC_Table_n [rad]. Used if PRC_Mode = 1. + default: [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] + PRC_R_Table: + type: array + items: + type: number + description: Table of turbine rating versus fine pitch (PRC_Pitch_Table), length should be PRC_Table_n, default is 1 [-]. Used if PRC_Mode = 1. + default: [1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1] TRA_ExclSpeed: type: number minimum: 0 @@ -1018,6 +1071,13 @@ properties: OL_Filename: type: string description: Input file with open loop timeseries (absolute path or relative to this file) + OL_BP_Mode: + description: Breakpoint mode for open loop control, 0 - indexed by time (default), 1 - indexed by wind speed + type: number + OL_BP_FiltFreq: + description: Natural frequency of 1st order filter on breakpoint for open loop control. 0 will skip filter. + type: number + default: 0 Ind_Breakpoint: type: number description: The column in OL_Filename that contains the breakpoint (time if OL_Mode > 0) @@ -1039,6 +1099,18 @@ properties: default: [0,0,0,0] items: type: number + Ind_R_Speed: + description: Index (column, 1-indexed) of power rating via speed offset + type: number + minimum: 0 + Ind_R_Torque: + description: Index (column, 1-indexed) of power rating via torque offset + type: number + minimum: 0 + Ind_R_Pitch: + description: Index (column, 1-indexed) of power rating via pitch offset + type: number + minimum: 0 Ind_CableControl: type: array items: diff --git a/rosco/toolbox/utilities.py b/rosco/toolbox/utilities.py index 15e4d550..30224f49 100644 --- a/rosco/toolbox/utilities.py +++ b/rosco/toolbox/utilities.py @@ -101,7 +101,7 @@ def write_DISCON(turbine, controller, param_file='DISCON.IN', txt_filename='Cp_C file.write('{0:<12d} ! PC_ControlMode - Blade pitch control mode {{0: No pitch, fix to fine pitch, 1: active PI blade pitch control}}\n'.format(int(rosco_vt['PC_ControlMode']))) file.write('{0:<12d} ! Y_ControlMode - Yaw control mode {{0: no yaw control, 1: yaw rate control, 2: yaw-by-IPC}}\n'.format(int(rosco_vt['Y_ControlMode']))) file.write('{0:<12d} ! SS_Mode - Setpoint Smoother mode {{0: no setpoint smoothing, 1: introduce setpoint smoothing}}\n'.format(int(rosco_vt['SS_Mode']))) - file.write('{0:<12d} ! PRC_Mode - Power reference tracking mode{{0: use standard rotor speed set points, 1: use PRC rotor speed setpoints}}\n'.format(int(rosco_vt['PRC_Mode']))) + file.write('{0:<12d} ! PRC_Mode - Power reference tracking mode{{0: power control disabled, 1: lookup table from wind speed to generator speed setpoints, 2: change speed, torque, pitch to control power}}\n'.format(int(rosco_vt['PRC_Mode']))) file.write('{0:<12d} ! WE_Mode - Wind speed estimator mode {{0: One-second low pass filtered hub height wind speed, 1: Immersion and Invariance Estimator, 2: Extended Kalman Filter}}\n'.format(int(rosco_vt['WE_Mode']))) file.write('{0:<12d} ! PS_Mode - Pitch saturation mode {{0: no pitch saturation, 1: implement pitch saturation}}\n'.format(int(rosco_vt['PS_Mode']))) file.write('{0:<12d} ! SD_Mode - Shutdown mode {{0: no shutdown procedure, 1: pitch to max pitch at shutdown}}\n'.format(int(rosco_vt['SD_Mode']))) @@ -184,6 +184,13 @@ def write_DISCON(turbine, controller, param_file='DISCON.IN', txt_filename='Cp_C file.write('{:<13.5f} ! SS_PCGain - Collective pitch controller setpoint smoother gain, [-].\n'.format(rosco_vt['SS_PCGain'])) file.write('\n') file.write('!------- POWER REFERENCE TRACKING --------------------------------------\n') + file.write('{:<11d} ! PRC_Comm - {}\n'.format(int(rosco_vt['PRC_Comm']), input_descriptions["PRC_Comm"])) + file.write('{:<13.5f} ! PRC_R_Torque - {}\n'.format(float(rosco_vt['PRC_R_Torque']), input_descriptions["PRC_R_Torque"])) + file.write('{:<13.5f} ! PRC_R_Speed - {}\n'.format(float(rosco_vt['PRC_R_Speed']), input_descriptions["PRC_R_Speed"])) + file.write('{:<13.5f} ! PRC_R_Pitch - {}\n'.format(float(rosco_vt['PRC_R_Pitch']), input_descriptions["PRC_R_Pitch"])) + file.write('{:<11d} ! PRC_Table_n - {}\n'.format(int(rosco_vt['PRC_Table_n']), input_descriptions["PRC_Table_n"])) + file.write('{} ! PRC_R_Table - {}\n'.format(write_array(rosco_vt["PRC_R_Table"]), input_descriptions["PRC_R_Table"])) + file.write('{} ! PRC_Pitch_Table - {}\n'.format(write_array(rosco_vt["PRC_Pitch_Table"]), input_descriptions["PRC_Pitch_Table"])) file.write('{:<11d} ! PRC_n - Number of elements in PRC_WindSpeeds and PRC_GenSpeeds array\n'.format(int(rosco_vt['PRC_n']))) file.write('{:<13.5f} ! PRC_LPF_Freq - {}\n'.format(float(rosco_vt['PRC_LPF_Freq']), input_descriptions["PRC_LPF_Freq"])) file.write('{} ! PRC_WindSpeeds - {}\n'.format(write_array(rosco_vt["PRC_WindSpeeds"]), input_descriptions["PRC_WindSpeeds"])) @@ -246,6 +253,8 @@ def write_DISCON(turbine, controller, param_file='DISCON.IN', txt_filename='Cp_C file.write('\n') file.write('!------- Open Loop Control -----------------------------------------------------\n') file.write('"{}" ! OL_Filename - Input file with open loop timeseries (absolute path or relative to this file)\n'.format(rosco_vt['OL_Filename'])) + file.write('{:<12d} ! OL_BP_Mode - {}]\n'.format(int(rosco_vt['OL_BP_Mode']),input_descriptions['OL_BP_Mode'])) + file.write('{:<12f} ! OL_BP_FiltFreq - {}\n'.format(float(rosco_vt['OL_BP_FiltFreq']),input_descriptions['OL_BP_FiltFreq'])) file.write('{0:<12d} ! Ind_Breakpoint - The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1)\n'.format(int(rosco_vt['Ind_Breakpoint']))) file.write('{} ! Ind_BldPitch - The columns in OL_Filename that contains the blade pitch (1,2,3) inputs in rad [array]\n'.format(' '.join([f'{int(ipb):3d}' for ipb in rosco_vt['Ind_BldPitch']]))) file.write('{0:<12d} ! Ind_GenTq - The column in OL_Filename that contains the generator torque in Nm\n'.format(int(rosco_vt['Ind_GenTq']))) @@ -254,6 +263,9 @@ def write_DISCON(turbine, controller, param_file='DISCON.IN', txt_filename='Cp_C file.write('{} ! {} - {}\n'.format(' '.join([f'{g:02.4f}' for g in rosco_vt["RP_Gains"]]),"RP_Gains",input_descriptions["RP_Gains"])) file.write('{} ! Ind_CableControl - The column(s) in OL_Filename that contains the cable control inputs in m [Used with CC_Mode = 2, must be the same size as CC_Group_N]\n'.format(write_array(rosco_vt['Ind_CableControl'],'<4d'))) file.write('{} ! Ind_StructControl - The column(s) in OL_Filename that contains the structural control inputs [Used with StC_Mode = 2, must be the same size as StC_Group_N]\n'.format(write_array(rosco_vt['Ind_StructControl'],'<4d'))) + file.write('{:<12d} ! Ind_R_Speed - {}\n'.format(int(rosco_vt["Ind_R_Speed"]), input_descriptions["Ind_R_Speed"])) + file.write('{:<12d} ! Ind_R_Torque - {}\n'.format(int(rosco_vt["Ind_R_Torque"]), input_descriptions["Ind_R_Torque"])) + file.write('{:<12d} ! Ind_R_Pitch - {}\n'.format(int(rosco_vt["Ind_R_Pitch"]), input_descriptions["Ind_R_Pitch"])) file.write('\n') file.write('!------- Pitch Actuator Model -----------------------------------------------------\n') file.write('{:<014.5f} ! PA_CornerFreq - Pitch actuator bandwidth/cut-off frequency [rad/s]\n'.format(rosco_vt['PA_CornerFreq'])) @@ -603,13 +615,17 @@ def DISCON_dict(turbine, controller, txt_filename=None): DISCON_dict['Flp_MaxPit'] = controller.flp_maxpit # ------- Open Loop Control ------- DISCON_dict['OL_Filename'] = controller.OL_Filename + DISCON_dict['OL_BP_Mode'] = controller.OL_BP_Mode DISCON_dict['Ind_Breakpoint'] = controller.OL_Ind_Breakpoint DISCON_dict['Ind_BldPitch'] = controller.OL_Ind_BldPitch DISCON_dict['Ind_GenTq'] = controller.OL_Ind_GenTq DISCON_dict['Ind_YawRate'] = controller.OL_Ind_YawRate DISCON_dict['Ind_CableControl'] = controller.OL_Ind_CableControl DISCON_dict['Ind_StructControl'] = controller.OL_Ind_StructControl - DISCON_dict['Ind_Azimuth'] = controller.OL_Ind_Azimuth + DISCON_dict['Ind_Azimuth'] = controller.OL_Ind_Azimuth + DISCON_dict['Ind_R_Speed'] = controller.OL_Ind_R_Speed + DISCON_dict['Ind_R_Torque'] = controller.OL_Ind_R_Torque + DISCON_dict['Ind_R_Pitch'] = controller.OL_Ind_R_Pitch # ------- Pitch Actuator ------- DISCON_dict['PA_Mode'] = controller.PA_Mode