diff --git a/DISCON/DISCON_gwin32.dll b/DISCON/DISCON_gwin32.dll index 9e37f00c..52e897c7 100644 Binary files a/DISCON/DISCON_gwin32.dll and b/DISCON/DISCON_gwin32.dll differ diff --git a/Scripts/CompileDISCONHere.cmd b/Scripts/CompileDISCONHere.cmd index e44a8f52..0908863d 100644 --- a/Scripts/CompileDISCONHere.cmd +++ b/Scripts/CompileDISCONHere.cmd @@ -7,7 +7,13 @@ set "DISCONSourceDir=..\" DEL %DISCONSourceDir%\DISCON\DISCON_gwin32.dll :: Compile new .dll file +del /f /s /q Obj_win32 1>nul +rmdir /s /q Obj_win32 + cd %DISCONSourceDir%\Source mingw32-make.exe +del /f /s /q Obj_win32 1>nul +rmdir /s /q Obj_win32 + pause diff --git a/Source/Constants.f90 b/Source/Constants.f90 new file mode 100644 index 00000000..b1cb78cf --- /dev/null +++ b/Source/Constants.f90 @@ -0,0 +1,5 @@ +MODULE Constants + REAL(4), PARAMETER :: RPS2RPM = 9.5492966 ! Factor to convert radians per second to revolutions per minute. + REAL(4), PARAMETER :: R2D = 57.295780 ! Factor to convert radians to degrees. + REAL(4), PARAMETER :: PI = 3.14159265359 ! Mathematical constant pi +END MODULE Constants \ No newline at end of file diff --git a/Source/Controllers.f90 b/Source/Controllers.f90 new file mode 100644 index 00000000..2f61e493 --- /dev/null +++ b/Source/Controllers.f90 @@ -0,0 +1,259 @@ +MODULE Controllers + + USE, INTRINSIC :: ISO_C_Binding + USE FunctionToolbox + USE Filters + + IMPLICIT NONE + +CONTAINS + SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst) + + USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances + + ! Local Variables: + REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. + INTEGER(4) :: K ! Loops through blades. + + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + + !.............................................................................................................................. + ! Pitch control + !.............................................................................................................................. + ! Set the pitch override to yes + avrSWAP(55) = 0.0 ! Pitch override: 0=yes + + IF (CntrPar%VS_ControlMode == 0 .AND. LocalVar%GenTrq >= CntrPar%PC_RtTq99) THEN + LocalVar%PC_MaxPitVar = CntrPar%PC_MaxPit + ELSEIF (CntrPar%VS_ControlMode == 1 .AND. LocalVar%GenTrqAr >= CntrPar%VS_GenTrqArSatMax*0.99) THEN + LocalVar%PC_MaxPitVar = CntrPar%PC_MaxPit + ELSE + LocalVar%PC_MaxPitVar = CntrPar%PC_SetPnt + END IF + + ! Compute the gain scheduling correction factor based on the previously + ! commanded pitch angle for blade 1: + LocalVar%PC_KP = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_kp, LocalVar%PC_PitComT) + LocalVar%PC_KI = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_ki, LocalVar%PC_PitComT) + LocalVar%PC_KD = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_kd, LocalVar%PC_PitComT) + LocalVar%PC_TF = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_tf, LocalVar%PC_PitComT) + + ! Compute the current speed error and its integral w.r.t. time; saturate the + ! integral term using the pitch angle limits: + LocalVar%PC_SpdErr = CntrPar%PC_RefSpd - LocalVar%GenSpeedF ! Speed error + LocalVar%PC_PwrErr = CntrPar%VS_RtPwr - LocalVar%VS_GenPwr ! Power error + LocalVar%Y_MErr = LocalVar%Y_M + CntrPar%Y_MErrSet ! Yaw-alignment error + + ! Compute the pitch commands associated with the proportional and integral + ! gains: + LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_SetPnt, LocalVar%PC_MaxPitVar, LocalVar%DT, CntrPar%PC_SetPnt, .FALSE., 2) ! + DFController(LocalVar%PC_SpdErr, LocalVar%PC_KD, LocalVar%PC_TF, LocalVar%DT, 1) + IF (CntrPar%VS_ControlMode == 1) THEN + LocalVar%PC_PitComT = LocalVar%PC_PitComT + PIController(LocalVar%PC_PwrErr, -4.0E-09, -4.0E-09, CntrPar%PC_SetPnt, LocalVar%PC_MaxPitVar, LocalVar%DT, CntrPar%PC_SetPnt, .FALSE., 5) + END IF + + ! Individual pitch control + IF ((CntrPar%IPC_ControlMode == 1) .OR. (CntrPar%Y_ControlMode == 2)) THEN + CALL IPC(CntrPar, LocalVar, objInst) + ELSE + LocalVar%IPC_PitComF = 0.0 ! THIS IS AN ARRAY!! + END IF + + ! Combine and saturate all pitch commands: + DO K = 1,LocalVar%NumBl ! Loop through all blades, add IPC contribution and limit pitch rate + LocalVar%PC_PitComT_IPC(K) = LocalVar%PC_PitComT + LocalVar%IPC_PitComF(K) ! Add the individual pitch command + LocalVar%PC_PitComT_IPC(K) = saturate(LocalVar%PC_PitComT_IPC(K), CntrPar%PC_MinPit, CntrPar%PC_MaxPit) ! Saturate the overall command using the pitch angle limits + + ! PitCom(K) = ratelimit(LocalVar%PC_PitComT_IPC(K), LocalVar%BlPitch(K), PC_MinRat, PC_MaxRat, LocalVar%DT) ! Saturate the overall command of blade K using the pitch rate limit + LocalVar%PitCom(K) = saturate(LocalVar%PC_PitComT_IPC(K), CntrPar%PC_MinPit, CntrPar%PC_MaxPit) ! Saturate the overall command using the pitch angle limits + LocalVar%PitCom(K) = LPFilter(LocalVar%PitCom(K), LocalVar%DT, CntrPar%CornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF) + END DO + + ! Command the pitch demanded from the last + ! call to the controller (See Appendix A of Bladed User's Guide): + avrSWAP(42) = LocalVar%PitCom(1) ! Use the command angles of all blades if using individual pitch + avrSWAP(43) = LocalVar%PitCom(2) ! " + avrSWAP(44) = LocalVar%PitCom(3) ! " + avrSWAP(45) = LocalVar%PitCom(1) ! Use the command angle of blade 1 if using collective pitch + END SUBROUTINE PitchControl + + SUBROUTINE VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst) + + USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances + + REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. + + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + + !.............................................................................................................................. + ! VARIABLE-SPEED TORQUE CONTROL: + !.............................................................................................................................. + avrSWAP(35) = 1.0 ! Generator contactor status: 1=main (high speed) variable-speed generator + avrSWAP(56) = 0.0 ! Torque override: 0=yes + + ! Filter the HSS (generator) speed measurement: + ! Apply Low-Pass Filter + LocalVar%GenSpeedF = SecLPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%CornerFreq, 0.7, LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! This is the first instance of a second order LPFilter + + ! Compute the generator torque, which depends on which region we are in: + + LocalVar%VS_SpdErrAr = CntrPar%VS_RtSpd - LocalVar%GenSpeedF ! Current speed error - Above-rated PI-control + LocalVar%VS_SpdErrBr = CntrPar%VS_MinOM - LocalVar%GenSpeedF ! Current speed error - Below-rated PI-control + IF (LocalVar%PC_PitComT >= CntrPar%VS_Rgn3MP) THEN ! We are in region 3 + LocalVar%GenTrqAr = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_Rgn2MaxTq, CntrPar%VS_GenTrqArSatMax, LocalVar%DT, CntrPar%VS_GenTrqArSatMax, .TRUE., 1) + LocalVar%GenTrqBr = PIController(LocalVar%VS_SpdErrBr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, CntrPar%VS_Rgn2MinTq, LocalVar%DT, CntrPar%VS_Rgn2MinTq, .TRUE., 4) + IF (CntrPar%VS_ControlMode == 1) THEN ! Constant power tracking + LocalVar%GenTrq = CntrPar%VS_RtPwr/LocalVar%GenSpeedF + ELSE ! Constant torque tracking + LocalVar%GenTrq = CntrPar%VS_RtTq + END IF + ELSE + LocalVar%GenTrqAr = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_Rgn2MaxTq, CntrPar%VS_GenTrqArSatMax, LocalVar%DT, CntrPar%VS_Rgn2MaxTq, .FALSE., 1) + LocalVar%GenTrqBr = PIController(LocalVar%VS_SpdErrBr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, CntrPar%VS_Rgn2MinTq, LocalVar%DT, CntrPar%VS_Rgn2MinTq, .FALSE., 4) + IF (LocalVar%GenTrqAr >= CntrPar%VS_Rgn2MaxTq*1.01) THEN + LocalVar%GenTrq = LocalVar%GenTrqAr + CONTINUE + ELSEIF (LocalVar%GenTrqBr <= CntrPar%VS_Rgn2MinTq*0.99) THEN ! We are in region 1 1/2 + LocalVar%GenTrq = LocalVar%GenTrqBr + CONTINUE + ELSEIF (LocalVar%GenSpeedF < CntrPar%VS_MaxOM) THEN ! We are in region 2 - optimal torque is proportional to the square of the generator speed + LocalVar%GenTrq = CntrPar%VS_Rgn2K*LocalVar%GenSpeedF*LocalVar%GenSpeedF + ELSE ! We are in region 2 1/2 - simple induction generator transition region + LocalVar%GenTrq = CntrPar%VS_Rgn2MaxTq + END IF + END IF + + ! Saturate the commanded torque using the maximum torque limit: + LocalVar%GenTrq = MIN(LocalVar%GenTrq, CntrPar%VS_MaxTq) ! Saturate the command using the maximum torque limit + + ! Saturate the commanded torque using the torque rate limit: + IF (LocalVar%iStatus == 0) LocalVar%VS_LastGenTrq = LocalVar%GenTrq ! Initialize the value of LocalVar%VS_LastGenTrq on the first pass only + LocalVar%GenTrq = ratelimit(LocalVar%GenTrq, LocalVar%VS_LastGenTrq, -CntrPar%VS_MaxRat, CntrPar%VS_MaxRat, LocalVar%DT) ! Saturate the command using the torque rate limit + + ! Reset the value of LocalVar%VS_LastGenTrq to the current values: + LocalVar%VS_LastGenTrq = LocalVar%GenTrq + + ! Set the generator contactor status, avrSWAP(35), to main (high speed) + ! variable-speed generator, the torque override to yes, and command the + ! generator torque (See Appendix A of Bladed User's Guide): + avrSWAP(47) = LocalVar%VS_LastGenTrq ! Demanded generator torque + END SUBROUTINE VariableSpeedControl + + SUBROUTINE YawRateControl(avrSWAP, CntrPar, LocalVar, objInst) + + USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances + + REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. + + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + + !.............................................................................................................................. + ! Yaw control + !.............................................................................................................................. + + IF (CntrPar%Y_ControlMode == 1) THEN + avrSWAP(29) = 0 ! Yaw control parameter: 0 = yaw rate control + IF (LocalVar%Time >= LocalVar%Y_YawEndT) THEN ! Check if the turbine is currently yawing + avrSWAP(48) = 0.0 ! Set yaw rate to zero + + LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .FALSE., objInst%instLPF) ! Fast low pass filtered yaw error with a frequency of 1 + LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .FALSE., objInst%instLPF) ! Slow low pass filtered yaw error with a frequency of 1/60 + + LocalVar%Y_AccErr = LocalVar%Y_AccErr + LocalVar%DT*SIGN(LocalVar%Y_ErrLPFFast**2, LocalVar%Y_ErrLPFFast) ! Integral of the fast low pass filtered yaw error + + IF (ABS(LocalVar%Y_AccErr) >= CntrPar%Y_ErrThresh) THEN ! Check if accumulated error surpasses the threshold + LocalVar%Y_YawEndT = ABS(LocalVar%Y_ErrLPFSlow/CntrPar%Y_Rate) + LocalVar%Time ! Yaw to compensate for the slow low pass filtered error + END IF + ELSE + avrSWAP(48) = SIGN(CntrPar%Y_Rate, LocalVar%Y_MErr) ! Set yaw rate to predefined yaw rate, the sign of the error is copied to the rate + LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .TRUE., objInst%instLPF) ! Fast low pass filtered yaw error with a frequency of 1 + LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .TRUE., objInst%instLPF) ! Slow low pass filtered yaw error with a frequency of 1/60 + LocalVar%Y_AccErr = 0.0 ! " + END IF + END IF + END SUBROUTINE YawRateControl + + SUBROUTINE IPC(CntrPar, LocalVar, objInst) + !------------------------------------------------------------------------------------------------------------------------------- + ! Individual pitch control subroutine + ! + ! Variable declaration and initialization + !------------------------------------------------------------------------------------------------------------------------------ + USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances + + ! Local variables + REAL(4) :: PitComIPC(3) + INTEGER(4) :: K ! Integer used to loop through turbine blades + REAL(4) :: axisTilt, axisYaw, axisYawF ! Direct axis and quadrature axis outputted by Coleman transform + REAL(4), SAVE :: IntAxisTilt, IntAxisYaw ! Integral of the direct axis and quadrature axis + REAL(4) :: IntAxisYawIPC ! IPC contribution with yaw-by-IPC component + REAL(4) :: Y_MErrF, Y_MErrF_IPC ! Unfiltered and filtered yaw alignment error [rad] + REAL(4) :: PitComIPC_woYaw(3) + + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + + !------------------------------------------------------------------------------------------------------------------------------ + ! Body + !------------------------------------------------------------------------------------------------------------------------------ + ! Calculates the commanded pitch angles. + ! NOTE: if it is required for this subroutine to be used multiple times (for 1p and 2p IPC for example), the saved variables + ! IntAxisTilt and IntAxisYaw need to be modified so that they support multiple instances (see LPFilter in the Filters module). + !------------------------------------------------------------------------------------------------------------------------------ + ! Filter rootMOOPs with notch filter + + !DO K = 1,LocalVar%NumBl + ! Instances 1-3 of the Notch Filter are reserved for this routine. + ! rootMOOPF(K) = LocalVar%rootMOOP(K) ! Notch filter currently not in use + !END DO + + ! Initialization + ! Set integrals to be 0 in the first time step + + IF(LocalVar%iStatus==0) THEN + IntAxisTilt = 0.0 + IntAxisYaw = 0.0 + END IF + + ! Pass rootMOOPs through the Coleman transform to get the direct and quadrature axis + CALL ColemanTransform(LocalVar%rootMOOP, LocalVar%Azimuth, axisTilt, axisYaw) + + ! High-pass filter the MBC yaw component and filter yaw alignment error, and compute the yaw-by-IPC contribution + IF (CntrPar%Y_ControlMode == 2) THEN + axisYawF = HPFilter(axisYaw, LocalVar%DT, CntrPar%IPC_omegaHP, LocalVar%iStatus, .FALSE., objInst%instHPF) + Y_MErrF = SecLPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%IPC_omegaLP, CntrPar%IPC_zetaLP, LocalVar%iStatus, .FALSE., objInst%instSecLPF) + Y_MErrF_IPC = PIController(Y_MErrF, CntrPar%Y_IPC_KP(1), CntrPar%Y_IPC_KI(1), -100.0, 100.0, LocalVar%DT, 0.0, .FALSE., 3) + ELSE + axisYawF = axisYaw + Y_MErrF = 0.0 + END IF + + ! Integrate the signal and multiply with the IPC gain + IF (CntrPar%IPC_ControlMode == 1) THEN + IntAxisTilt = IntAxisTilt + LocalVar%DT * CntrPar%IPC_KI * axisTilt + IntAxisYaw = IntAxisYaw + LocalVar%DT * CntrPar%IPC_KI * axisYawF + ELSE + IntAxisTilt = 0.0 + IntAxisYaw = 0.0 + END IF + + ! Add the yaw-by-IPC contribution + IntAxisYawIPC = IntAxisYaw + Y_MErrF_IPC + + ! Pass direct and quadrature axis through the inverse Coleman transform to get the commanded pitch angles + CALL ColemanTransformInverse(IntAxisTilt, IntAxisYawIPC, LocalVar%Azimuth, CntrPar%IPC_phi, PitComIPC) + + ! Filter PitComIPC with second order low pass filter + DO K = 1,LocalVar%NumBl + ! Instances 1-3 of the Second order Low-Pass Filter are reserved for this routine. + ! LocalVar%IPC_PitComF(K) = SecLPFilter(PitComIPC(K), LocalVar%DT, CntrPar%IPC_omegaLP, CntrPar%IPC_zetaLP, LocalVar%iStatus, K) + LocalVar%IPC_PitComF(K) = PitComIPC(K) + END DO + END SUBROUTINE IPC +END MODULE Controllers \ No newline at end of file diff --git a/Source/DISCON.f90 b/Source/DISCON.f90 index 8ba5a47b..d23dbe9d 100644 --- a/Source/DISCON.f90 +++ b/Source/DISCON.f90 @@ -1,24 +1,14 @@ !======================================================================= -! SUBROUTINE DISCON (avrSWAP, from_SC, to_SC, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME='DISCON') -SUBROUTINE DISCON (avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME='DISCON') +! SUBROUTINE DISCON(avrSWAP, from_SC, to_SC, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME='DISCON') +SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME='DISCON') +! DO NOT REMOVE or MODIFY LINES starting with "!DEC$" or "!GCC$" +! !DEC$ specifies attributes for IVF and !GCC$ specifies attributes for gfortran !DEC$ ATTRIBUTES DLLEXPORT :: DISCON - ! 06/09/2017 - - ! This Bladed-style DLL controller is used to implement a variable-speed - ! generator-torque controller, PI collective blade pitch controller, individual pitch - ! controller and yaw controller for the NREL Offshore 5MW baseline wind turbine. - ! This routine was extended by S.P. Mulders, J. Hoorneman and J. Govers of TU Delft. - ! The routine is based on the routine as written by J. Jonkman of NREL/NWTC. - - ! DO NOT REMOVE or MODIFY LINES starting with "!DEC$" or "!GCC$" - ! !DEC$ specifies attributes for IVF and !GCC$ specifies attributes for gfortran - USE, INTRINSIC :: ISO_C_Binding -USE :: ReadParameters -USE :: FunctionToolbox -USE :: Filters -USE DRC_Types, ONLY : ObjectInstances +USE :: DRC_Types +USE :: ReadSetParameters +USE :: Controllers IMPLICIT NONE #ifndef IMPLICIT_DLLEXPORT @@ -29,7 +19,7 @@ SUBROUTINE DISCON (avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAM ! Variable declaration and initialization !------------------------------------------------------------------------------------------------------------------------------ - ! Passed Variables: +! Passed Variables: !REAL(C_FLOAT), INTENT(IN) :: from_SC(*) ! DATA from the supercontroller !REAL(C_FLOAT), INTENT(INOUT) :: to_SC(*) ! DATA to the supercontroller @@ -38,392 +28,26 @@ SUBROUTINE DISCON (avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAM CHARACTER(KIND=C_CHAR), INTENT(IN) :: accINFILE(NINT(avrSWAP(50))) ! The name of the parameter input file CHARACTER(KIND=C_CHAR), INTENT(IN) :: avcOUTNAME(NINT(avrSWAP(51))) ! OUTNAME (Simulation RootName) CHARACTER(KIND=C_CHAR), INTENT(INOUT) :: avcMSG(NINT(avrSWAP(49))) ! MESSAGE (Message from DLL to simulation code [ErrMsg]) The message which will be displayed by the calling program if aviFAIL <> 0. +CHARACTER(SIZE(avcOUTNAME)-1) :: RootName ! a Fortran version of the input C string (not considered an array here) [subtract 1 for the C null-character] +CHARACTER(SIZE(avcMSG)-1) :: ErrMsg ! a Fortran version of the C string argument (not considered an array here) [subtract 1 for the C null-character] - ! Types - -TYPE(ObjectInstances) :: objInst - - ! Local Variables: - -REAL(4), SAVE :: LastGenTrq ! Commanded electrical generator torque the last time the controller was called, [Nm]. -REAL(4), SAVE :: PitComT ! Total command pitch based on the sum of the proportional and integral terms, [rad]. -REAL(4), SAVE :: PitComT_IPC(3) ! Total command pitch based on the sum of the proportional and integral terms, including IPC term [rad]. -REAL(4), SAVE :: Y_AccErr ! Accumulated yaw error [rad]. -REAL(4), SAVE :: Y_YawEndT ! Yaw end time, [s]. Indicates the time up until which yaw is active with a fixed rate -REAL(4), SAVE :: testValue ! TestValue - -INTEGER(4) :: I ! Generic index. -INTEGER(4) :: instLPF ! Instance counter for all first-order low-pass filters -INTEGER(4) :: K ! Loops through blades. -INTEGER(4), PARAMETER :: UnDb = 85 ! I/O unit for the debugging information -INTEGER(4), PARAMETER :: UnDb2 = 86 ! I/O unit for the debugging information - -LOGICAL(1), PARAMETER :: DbgOut = .FALSE. ! Flag to indicate whether to output debugging information - -CHARACTER(1), PARAMETER :: Tab = CHAR(9) ! The tab character. -CHARACTER(25), PARAMETER :: FmtDat = "(F8.3,99('"//Tab//"',ES10.3E2,:)) " ! The format of the debugging data - -CHARACTER(SIZE(avcOUTNAME)-1) :: RootName ! a Fortran version of the input C string (not considered an array here) [subtract 1 for the C null-character] -CHARACTER(SIZE(avcMSG)-1) :: ErrMsg ! a Fortran version of the C string argument (not considered an array here) [subtract 1 for the C null-character] - - - - ! Read avrSWAP array into derived types/variables -CALL ReadAvrSWAP(avrSWAP) - - ! Initialize aviFAIL to 0: -aviFAIL = 0 - - ! Initialize all filter instance counters at 1 -objInst%instLPF = 1 -objInst%instSecLPF = 1 -objInst%instHPF = 1 -objInst%instNotchSlopes = 1 -objInst%instNotch = 1 - - ! Read any External Controller Parameters specified in the User Interface - ! and initialize variables: -IF (LocalVar%iStatus == 0) THEN ! .TRUE. if we're on the first call to the DLL - - ! Inform users that we are using this user-defined routine: - aviFAIL = 1 - ErrMsg = ' '//NEW_LINE('A')// & - 'Running the Delft Research Controller (DRC) '//NEW_LINE('A')// & - 'A wind turbine controller for use in the scientific field '//NEW_LINE('A')// & - 'Written by S.P. Mulders, Jan-Willem van Wingerden '//NEW_LINE('A')// & - 'Delft University of Technology, The Netherlands '//NEW_LINE('A')// & - 'Visit our GitHub-page to contribute to this project: '//NEW_LINE('A')// & - 'https://github.com/TUDelft-DataDrivenControl ' - - CALL ReadControlParameterFileSub() - - ! Initialize testValue (debugging variable) - ! testValue = 0.4 - - ! Initialize the SAVEd variables: - ! NOTE: LastGenTrq, though SAVEd, is initialized in the torque controller - ! below for simplicity, not here. - LocalVar%PitCom = LocalVar%BlPitch ! This will ensure that the variable speed controller picks the correct control region and the pitch controller picks the correct gain on the first call - Y_AccErr = 0.0 ! This will ensure that the accumulated yaw error starts at zero - Y_YawEndT = -1.0 ! This will ensure that the initial yaw end time is lower than the actual time to prevent initial yawing - - !.............................................................................................................................. - ! Check validity of input parameters: - !.............................................................................................................................. - - IF (CntrPar%CornerFreq <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'CornerFreq must be greater than zero.' - ENDIF - - IF (LocalVar%DT <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'DT must be greater than zero.' - ENDIF - - IF (CntrPar%VS_CtInSp < 0.0) THEN - aviFAIL = -1 - ErrMsg = 'VS_CtInSp must not be negative.' - ENDIF - - IF (CntrPar%VS_MinOM <= CntrPar%VS_CtInSp) THEN - aviFAIL = -1 - ErrMsg = 'VS_MinOM must be greater than VS_CtInSp.' - ENDIF - - IF (CntrPar%VS_MaxRat <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'VS_MaxRat must be greater than zero.' - ENDIF - - IF (CntrPar%VS_RtTq < 0.0) THEN - aviFAIL = -1 - ErrMsg = 'VS_RtTw must not be negative.' - ENDIF - - IF (CntrPar%VS_Rgn2K < 0.0) THEN - aviFAIL = -1 - ErrMsg = 'VS_Rgn2K must not be negative.' - ENDIF - - IF (CntrPar%VS_MaxTq < CntrPar%VS_RtTq) THEN - aviFAIL = -1 - ErrMsg = 'VS_RtTq must not be greater than VS_MaxTq.' - ENDIF - - IF (CntrPar%VS_KP(1) > 0.0) THEN - aviFAIL = -1 - ErrMsg = 'VS_KP must be greater than zero.' - ENDIF - - IF (CntrPar%VS_KI(1) > 0.0) THEN - aviFAIL = -1 - ErrMsg = 'VS_KI must be greater than zero.' - ENDIF - - IF (CntrPar%PC_RefSpd <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'PC_RefSpd must be greater than zero.' - ENDIF - - IF (CntrPar%PC_MaxRat <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'PC_MaxRat must be greater than zero.' - ENDIF - - IF (CntrPar%PC_MinPit >= CntrPar%PC_MaxPit) THEN - aviFAIL = -1 - ErrMsg = 'PC_MinPit must be less than PC_MaxPit.' - ENDIF - - IF (CntrPar%IPC_KI <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'IPC_KI must be greater than zero.' - ENDIF - - IF (CntrPar%IPC_omegaLP <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'IPC_omegaLP must be greater than zero.' - ENDIF - - IF (CntrPar%IPC_omegaNotch <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'IPC_omegaNotch must be greater than zero.' - ENDIF - - IF (CntrPar%IPC_phi <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'IPC_phi must be greater than zero.' - ENDIF - - IF (CntrPar%IPC_zetaLP <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'IPC_zetaLP must be greater than zero.' - ENDIF - - IF (CntrPar%IPC_zetaNotch <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'IPC_zetaNotch must be greater than zero.' - ENDIF - - IF (CntrPar%Y_ErrThresh <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'Y_ErrThresh must be greater than zero.' - ENDIF - - IF (CntrPar%Y_Rate <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'CntrPar%Y_Rate must be greater than zero.' - ENDIF - - IF (CntrPar%Y_omegaLPFast <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'Y_omegaLPFast must be greater than zero.' - ENDIF - - IF (CntrPar%Y_omegaLPSlow <= 0.0) THEN - aviFAIL = -1 - ErrMsg = 'Y_omegaLPSlow must be greater than zero.' - ENDIF - - !.............................................................................................................................. - ! Initializing debug file - !.............................................................................................................................. - - ! If we're debugging, open the debug file and write the header: - IF (CntrPar%LoggingLevel > 0) THEN - OPEN (UnDb, FILE=TRIM(RootName)//'.dbg', STATUS='REPLACE') - WRITE (UnDb,'(A)') ' LocalVar%Time ' //Tab//'PitComT ' //Tab//'LocalVar%PC_SpdErr ' //Tab//'LocalVar%PC_KP ' //Tab//'LocalVar%PC_KI ' //Tab//'LocalVar%Y_M ' //Tab//'LocalVar%rootMOOP(1) '//Tab//'VS_RtPwr '//Tab//'LocalVar%GenTrq' - WRITE (UnDb,'(A)') ' (sec) ' //Tab//'(rad) ' //Tab//'(rad/s) '//Tab//'(-) ' //Tab//'(-) ' //Tab//'(rad) ' //Tab//'(?) ' //Tab//'(W) '//Tab//'(Nm) ' - END IF - - IF (CntrPar%LoggingLevel > 1) THEN - OPEN(UnDb2, FILE=TRIM(RootName)//'.dbg2', STATUS='REPLACE') - WRITE(UnDb2,'(/////)') - WRITE(UnDb2,'(A,85("'//Tab//'AvrSWAP(",I2,")"))') 'LocalVar%Time ', (i,i=1,85) - WRITE(UnDb2,'(A,85("'//Tab//'(-)"))') '(s)' - END IF - -ENDIF +TYPE(ControlParameters), SAVE :: CntrPar +TYPE(LocalVariables), SAVE :: LocalVar +TYPE(ObjectInstances), SAVE :: objInst !------------------------------------------------------------------------------------------------------------------------------ ! Main control calculations !------------------------------------------------------------------------------------------------------------------------------ - +! Read avrSWAP array into derived types/variables +CALL ReadAvrSWAP(avrSWAP, LocalVar) +CALL SetParameters(avrSWAP, aviFAIL, ErrMsg, SIZE(avcMSG), CntrPar, LocalVar, objInst) IF ((LocalVar%iStatus >= 0) .AND. (aviFAIL >= 0)) THEN ! Only compute control calculations if no error has occurred and we are not on the last time step - ! Abort if the user has not requested a pitch angle actuator (See Appendix A - ! of Bladed User's Guide): - IF (NINT(avrSWAP(10)) /= 0) THEN ! .TRUE. if a pitch angle actuator hasn't been requested - aviFAIL = -1 - ErrMsg = 'Pitch angle actuator not requested.' - ENDIF - - ! Set unused outputs to zero (See Appendix A of Bladed User's Guide): - avrSWAP(36) = 0.0 ! Shaft brake status: 0=off - avrSWAP(41) = 0.0 ! Demanded yaw actuator torque - avrSWAP(46) = 0.0 ! Demanded pitch rate (Collective pitch) - avrSWAP(65) = 0.0 ! Number of variables returned for logging - avrSWAP(72) = 0.0 ! Generator start-up resistance - avrSWAP(79) = 0.0 ! Request for loads: 0=none - avrSWAP(80) = 0.0 ! Variable slip current status - avrSWAP(81) = 0.0 ! Variable slip current demand - - ! Filter the HSS (generator) speed measurement: - ! Apply Low-Pass Filter - LocalVar%GenSpeedF = SecLPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%CornerFreq, 0.7, LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! This is the first instance of a second order LPFilter - - ! Calculate yaw-alignment error - LocalVar%Y_MErr = LocalVar%Y_M + CntrPar%Y_MErrSet - - !.............................................................................................................................. - ! VARIABLE-SPEED TORQUE CONTROL: - !.............................................................................................................................. - - ! Compute the generator torque, which depends on which region we are in: - - LocalVar%VS_SpdErrAr = CntrPar%VS_RtSpd - LocalVar%GenSpeedF ! Current speed error - Above-rated PI-control - LocalVar%VS_SpdErrBr = CntrPar%VS_MinOM - LocalVar%GenSpeedF ! Current speed error - Below-rated PI-control - IF (PitComT >= CntrPar%VS_Rgn3MP) THEN ! We are in region 3 - LocalVar%GenTrqAr = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_Rgn2MaxTq, CntrPar%VS_GenTrqArSatMax, LocalVar%DT, CntrPar%VS_GenTrqArSatMax, .TRUE., 1) - LocalVar%GenTrqBr = PIController(LocalVar%VS_SpdErrBr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, CntrPar%VS_Rgn2MinTq, LocalVar%DT, CntrPar%VS_Rgn2MinTq, .TRUE., 4) - IF (CntrPar%VS_ControlMode == 1) THEN ! Constant power tracking - LocalVar%GenTrq = CntrPar%VS_RtPwr/LocalVar%GenSpeedF - ELSE ! Constant torque tracking - LocalVar%GenTrq = CntrPar%VS_RtTq - END IF - ELSE - LocalVar%GenTrqAr = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_Rgn2MaxTq, CntrPar%VS_GenTrqArSatMax, LocalVar%DT, CntrPar%VS_Rgn2MaxTq, .FALSE., 1) - LocalVar%GenTrqBr = PIController(LocalVar%VS_SpdErrBr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, CntrPar%VS_Rgn2MinTq, LocalVar%DT, CntrPar%VS_Rgn2MinTq, .FALSE., 4) - IF (LocalVar%GenTrqAr >= CntrPar%VS_Rgn2MaxTq*1.01) THEN - LocalVar%GenTrq = LocalVar%GenTrqAr - CONTINUE - ELSEIF (LocalVar%GenTrqBr <= CntrPar%VS_Rgn2MinTq*0.99) THEN ! We are in region 1 1/2 - LocalVar%GenTrq = LocalVar%GenTrqBr - CONTINUE - ELSEIF (LocalVar%GenSpeedF < CntrPar%VS_MaxOM) THEN ! We are in region 2 - optimal torque is proportional to the square of the generator speed - LocalVar%GenTrq = CntrPar%VS_Rgn2K*LocalVar%GenSpeedF*LocalVar%GenSpeedF - ELSE ! We are in region 2 1/2 - simple induction generator transition region - LocalVar%GenTrq = CntrPar%VS_Rgn2MaxTq - END IF - END IF - - ! Saturate the commanded torque using the maximum torque limit: - - LocalVar%GenTrq = MIN(LocalVar%GenTrq, CntrPar%VS_MaxTq) ! Saturate the command using the maximum torque limit - - ! Saturate the commanded torque using the torque rate limit: - IF (LocalVar%iStatus == 0) LastGenTrq = LocalVar%GenTrq ! Initialize the value of LastGenTrq on the first pass only - LocalVar%GenTrq = ratelimit(LocalVar%GenTrq, LastGenTrq, -CntrPar%VS_MaxRat, CntrPar%VS_MaxRat, LocalVar%DT) ! Saturate the command using the torque rate limit - - ! Reset the value of LastGenTrq to the current values: - LastGenTrq = LocalVar%GenTrq - - ! Set the generator contactor status, avrSWAP(35), to main (high speed) - ! variable-speed generator, the torque override to yes, and command the - ! generator torque (See Appendix A of Bladed User's Guide): - avrSWAP(35) = 1.0 ! Generator contactor status: 1=main (high speed) variable-speed generator - avrSWAP(56) = 0.0 ! Torque override: 0=yes - avrSWAP(47) = LastGenTrq ! Demanded generator torque - - !.............................................................................................................................. - ! Pitch control - !.............................................................................................................................. - - IF (CntrPar%VS_ControlMode == 0 .AND. LocalVar%GenTrq >= CntrPar%PC_RtTq99) THEN - LocalVar%PC_MaxPitVar = CntrPar%PC_MaxPit - ELSEIF (CntrPar%VS_ControlMode == 1 .AND. LocalVar%GenTrqAr >= CntrPar%VS_GenTrqArSatMax*0.99) THEN - LocalVar%PC_MaxPitVar = CntrPar%PC_MaxPit - ELSE - LocalVar%PC_MaxPitVar = CntrPar%PC_SetPnt - END IF - - ! Compute the gain scheduling correction factor based on the previously - ! commanded pitch angle for blade 1: - LocalVar%PC_KP = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_kp, PitComT) - LocalVar%PC_KI = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_ki, PitComT) - LocalVar%PC_KD = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_kd, PitComT) - LocalVar%PC_TF = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_tf, PitComT) - - ! Compute the current speed error and its integral w.r.t. time; saturate the - ! integral term using the pitch angle limits: - LocalVar%PC_SpdErr = CntrPar%PC_RefSpd - LocalVar%GenSpeedF ! Speed error - LocalVar%PC_PwrErr = CntrPar%VS_RtPwr - LocalVar%VS_GenPwr ! Power error - - ! Compute the pitch commands associated with the proportional and integral - ! gains: - ! PitComT = NotchFilter(PC_SpdErr, LocalVar%DT, 1.59, 0.01, 0.2, LocalVar%iStatus, 1) - PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_SetPnt, LocalVar%PC_MaxPitVar, LocalVar%DT, CntrPar%PC_SetPnt, .FALSE., 2) ! + DFController(LocalVar%PC_SpdErr, LocalVar%PC_KD, LocalVar%PC_TF, LocalVar%DT, 1) - IF (CntrPar%VS_ControlMode == 1) THEN - PitComT = PitComT + PIController(LocalVar%PC_PwrErr, -4.0E-09, -4.0E-09, CntrPar%PC_SetPnt, LocalVar%PC_MaxPitVar, LocalVar%DT, CntrPar%PC_SetPnt, .FALSE., 5) - END IF - - ! Individual pitch control - IF ((CntrPar%IPC_ControlMode == 1) .OR. (CntrPar%Y_ControlMode == 2)) THEN - CALL IPC(LocalVar, CntrPar, objInst) - ELSE - LocalVar%IPC_PitComF = 0.0 ! THIS IS AN ARRAY!! - END IF - - ! Combine and saturate all pitch commands: - DO K = 1,LocalVar%NumBl ! Loop through all blades, add IPC contribution and limit pitch rate - PitComT_IPC(K) = PitComT + LocalVar%IPC_PitComF(K) ! Add the individual pitch command - PitComT_IPC(K) = saturate(PitComT_IPC(K), CntrPar%PC_MinPit, CntrPar%PC_MaxPit) ! Saturate the overall command using the pitch angle limits - - ! PitCom(K) = ratelimit(PitComT_IPC(K), LocalVar%BlPitch(K), PC_MinRat, PC_MaxRat, LocalVar%DT) ! Saturate the overall command of blade K using the pitch rate limit - LocalVar%PitCom(K) = saturate(PitComT_IPC(K), CntrPar%PC_MinPit, CntrPar%PC_MaxPit) ! Saturate the overall command using the pitch angle limits - LocalVar%PitCom(K) = LPFilter(LocalVar%PitCom(K), LocalVar%DT, CntrPar%CornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF) - END DO - - ! Set the pitch override to yes and command the pitch demanded from the last - ! call to the controller (See Appendix A of Bladed User's Guide): - - avrSWAP(55) = 0.0 ! Pitch override: 0=yes - - avrSWAP(42) = LocalVar%PitCom(1) ! Use the command angles of all blades if using individual pitch - avrSWAP(43) = LocalVar%PitCom(2) ! " - avrSWAP(44) = LocalVar%PitCom(3) ! " - - avrSWAP(45) = LocalVar%PitCom(1) ! Use the command angle of blade 1 if using collective pitch - - !.............................................................................................................................. - ! Yaw control - !.............................................................................................................................. - - IF (CntrPar%Y_ControlMode == 1) THEN - avrSWAP(29) = 0 ! Yaw control parameter: 0 = yaw rate control - IF (LocalVar%Time >= Y_YawEndT) THEN ! Check if the turbine is currently yawing - avrSWAP(48) = 0.0 ! Set yaw rate to zero - - LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .FALSE., objInst%instLPF) ! Fast low pass filtered yaw error with a frequency of 1 - LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .FALSE., objInst%instLPF) ! Slow low pass filtered yaw error with a frequency of 1/60 - - Y_AccErr = Y_AccErr + LocalVar%DT*SIGN(LocalVar%Y_ErrLPFFast**2, LocalVar%Y_ErrLPFFast) ! Integral of the fast low pass filtered yaw error - - IF (ABS(Y_AccErr) >= CntrPar%Y_ErrThresh) THEN ! Check if accumulated error surpasses the threshold - Y_YawEndT = ABS(LocalVar%Y_ErrLPFSlow/CntrPar%Y_Rate) + LocalVar%Time ! Yaw to compensate for the slow low pass filtered error - END IF - ELSE - avrSWAP(48) = SIGN(CntrPar%Y_Rate, LocalVar%Y_MErr) ! Set yaw rate to predefined yaw rate, the sign of the error is copied to the rate - LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .TRUE., objInst%instLPF) ! Fast low pass filtered yaw error with a frequency of 1 - LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .TRUE., objInst%instLPF) ! Slow low pass filtered yaw error with a frequency of 1/60 - Y_AccErr = 0.0 ! " - END IF - END IF - !.............................................................................................................................. - ! Output debugging information if requested: - IF (CntrPar%LoggingLevel > 0) THEN - WRITE (UnDb,FmtDat) LocalVar%Time, PitComT, LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, LocalVar%Y_MErr, LocalVar%rootMOOP(1), CntrPar%VS_RtPwr, LocalVar%GenTrq - END IF - IF (CntrPar%LoggingLevel > 1) THEN - WRITE (UnDb2,FmtDat) LocalVar%Time, avrSWAP(1:85) - END IF - - -ENDIF + CALL VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst) + CALL PitchControl(avrSWAP, CntrPar, LocalVar, objInst) +! CALL YawRateControl(avrSWAP, CntrPar, LocalVar, objInst) +END IF avcMSG = TRANSFER(TRIM(ErrMsg)//C_NULL_CHAR, avcMSG, SIZE(avcMSG)) - RETURN - END SUBROUTINE DISCON diff --git a/Source/DRC_Types.f90 b/Source/DRC_Types.f90 index 83ff3770..9e04e5d5 100644 --- a/Source/DRC_Types.f90 +++ b/Source/DRC_Types.f90 @@ -2,109 +2,109 @@ MODULE DRC_Types IMPLICIT NONE -REAL(4), PARAMETER :: RPS2RPM = 9.5492966 ! Factor to convert radians per second to revolutions per minute. -REAL(4), PARAMETER :: R2D = 57.295780 ! Factor to convert radians to degrees. - - TYPE, PUBLIC :: ControlParameters - REAL(4) :: CornerFreq ! Corner frequency (-3dB point) in the first-order low-pass filter, [rad/s] - INTEGER(4) :: LoggingLevel ! 0 = write no debug files, 1 = write standard output .dbg-file, 2 = write standard output .dbg-file and complete avrSWAP-array .dbg2-file - REAL(4) :: IPC_KI ! Integral gain for the individual pitch controller, [-]. 8E-10 - INTEGER(4) :: IPC_ControlMode ! Turn Individual Pitch Control (IPC) for fatigue load reductions (pitch contribution) on = 1/off = 0 - REAL(4) :: IPC_omegaHP ! High-pass filter cut-in frequency used to separate yaw-by-IPC contribution from blade load reduction contribution, [rad/s]. - REAL(4) :: IPC_omegaLP ! Low-pass filter corner frequency for the individual pitch controller, [rad/s]. - REAL(4) :: IPC_omegaNotch ! Notch filter corner frequency for the individual pitch controller, [rad/s]. - REAL(4) :: IPC_phi ! Phase offset added to the azimuth angle for the individual pitch controller, [rad]. - REAL(4) :: IPC_zetaHP ! High-pass filter damping value, [-]. - REAL(4) :: IPC_zetaLP ! Low-pass filter damping factor for the individual pitch controller, [-]. - REAL(4) :: IPC_zetaNotch ! Notch filter damping factor for the individual pitch controller, [-]. - INTEGER(4) :: PC_GS_n ! Amount of gain-scheduling table entries - REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_angles ! Gain-schedule table: pitch angles - REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_kp ! Gain-schedule table: pitch controller kp gains - REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_ki ! Gain-schedule table: pitch controller ki gains - REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_kd ! Gain-schedule table: pitch controller kd gains - REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_tf ! Gain-schedule table: pitch controller tf gains (derivative filter) - REAL(4) :: PC_MaxPit ! Maximum physical pitch limit, [rad]. - REAL(4) :: PC_MinPit ! Minimum physical pitch limit, [rad]. - REAL(4) :: PC_MaxRat ! Maximum pitch rate (in absolute value) in pitch controller, [rad/s]. - REAL(4) :: PC_MinRat ! Minimum pitch rate (in absolute value) in pitch controller, [rad/s]. - REAL(4) :: PC_RefSpd ! Desired (reference) HSS speed for pitch controller, [rad/s]. - REAL(4) :: PC_SetPnt ! Record 5: Below-rated pitch angle set-point (deg) [used only with Bladed Interface] - REAL(4) :: PC_Switch ! Angle above lowest minimum pitch angle for switch [rad] - INTEGER(4) :: VS_ControlMode ! Generator torque control mode in above rated conditions, 0 = constant torque / 1 = constant power - REAL(4) :: VS_CtInSp ! Transitional generator speed (HSS side) between regions 1 and 1 1/2, [rad/s]. - REAL(4) :: VS_GenTrqArSatMax ! Above rated generator torque PI control saturation, [Nm] -- 212900 - REAL(4) :: VS_MaxOM ! Optimal mode maximum speed, [rad/s]. - REAL(4) :: VS_MaxRat ! Maximum torque rate (in absolute value) in torque controller, [Nm/s]. - REAL(4) :: VS_MaxTq ! Maximum generator torque in Region 3 (HSS side), [Nm]. -- chosen to be 10% above VS_RtTq - REAL(4) :: VS_MinTq ! Minimum generator (HSS side), [Nm]. - REAL(4) :: VS_MinOM ! Optimal mode minimum speed, [rad/s] - REAL(4) :: VS_Rgn2K ! Generator torque constant in Region 2 (HSS side), N-m/(rad/s)^2 - REAL(4) :: VS_RtPwr ! Wind turbine rated power [W] - REAL(4) :: VS_RtTq ! Rated torque, [Nm]. - REAL(4) :: VS_RtSpd ! Rated generator speed [rad/s] - INTEGER(4) :: VS_n ! Number of controller gains - REAL(4), DIMENSION(:), ALLOCATABLE :: VS_KP ! Proportional gain for generator PI torque controller, used in the transitional 2.5 region - REAL(4), DIMENSION(:), ALLOCATABLE :: VS_KI ! Integral gain for generator PI torque controller, used in the transitional 2.5 region - INTEGER(4) :: Y_ControlMode ! Yaw control mode: (0 = no yaw control, 1 = yaw rate control, 2 = yaw-by-IPC) - REAL(4) :: Y_ErrThresh ! Error threshold [rad]. Turbine begins to yaw when it passes this. (104.71975512) -- 1.745329252 - INTEGER(4) :: Y_IPC_n ! Number of controller gains (yaw-by-IPC) - REAL(4), DIMENSION(:), ALLOCATABLE :: Y_IPC_KP ! Yaw-by-IPC proportional controller gain Kp - REAL(4), DIMENSION(:), ALLOCATABLE :: Y_IPC_KI ! Yaw-by-IPC integral controller gain Ki - REAL(4) :: Y_MErrSet ! Yaw alignment error, setpoint [rad] - REAL(4) :: Y_omegaLPFast ! Corner frequency fast low pass filter, 1.0 [Hz] - REAL(4) :: Y_omegaLPSlow ! Corner frequency slow low pass filter, 1/60 [Hz] - REAL(4) :: Y_Rate ! Yaw rate [rad/s] - - REAL(4) :: PC_RtTq99 ! 99% of the rated torque value, using for switching between pitch and torque control, [Nm]. - REAL(4) :: VS_Rgn2MaxTq ! Maximum torque at the end of the below-rated region 2, [Nm] - REAL(4) :: VS_Rgn2MinTq ! Minimum torque at the beginning of the below-rated region 2, [Nm] - REAL(4) :: VS_Rgn3MP ! Minimum pitch angle at which the torque is computed as if we are in region 3 regardless of the generator speed, [rad]. - END TYPE ControlParameters - - TYPE, PUBLIC :: LocalVariables - ! From avrSWAP - INTEGER(4) :: iStatus - REAL(4) :: Time - REAL(4) :: DT - REAL(4) :: VS_GenPwr - REAL(4) :: GenSpeed - REAL(4) :: Y_M - REAL(4) :: HorWindV - REAL(4) :: rootMOOP(3) - REAL(4) :: BlPitch(3) - REAL(4) :: Azimuth - INTEGER(4) :: NumBl - - ! Internal controller variables - REAL(4) :: GenSpeedF ! Filtered HSS (generator) speed [rad/s]. - REAL(4) :: GenTrq ! Electrical generator torque, [Nm]. - REAL(4) :: GenTrqAr ! Electrical generator torque, for above-rated PI-control [Nm]. - REAL(4) :: GenTrqBr ! Electrical generator torque, for below-rated PI-control [Nm]. - REAL(4) :: IPC_PitComF(3) ! Commanded pitch of each blade as calculated by the individual pitch controller, F stands for low pass filtered, [rad]. - REAL(4) :: PC_KP ! Proportional gain for pitch controller at rated pitch (zero), [s]. - REAL(4) :: PC_KI ! Integral gain for pitch controller at rated pitch (zero), [-]. - REAL(4) :: PC_KD ! Differential gain for pitch controller at rated pitch (zero), [-]. - REAL(4) :: PC_TF ! First-order filter parameter for derivative action - REAL(4) :: PC_MaxPitVar ! Maximum pitch setting in pitch controller (variable) [rad]. - REAL(4) :: PC_PwrErr ! Power error with respect to rated power [W] - REAL(4) :: PC_SpdErr ! Current speed error (pitch control) [rad/s]. - REAL(4) :: PitCom(3) ! Commanded pitch of each blade the last time the controller was called, [rad]. - REAL(4) :: VS_SpdErrAr ! Current speed error (generator torque control) [rad/s]. - REAL(4) :: VS_SpdErrBr ! Current speed error (generator torque control) [rad/s]. - REAL(4) :: Y_ErrLPFFast ! Filtered yaw error by fast low pass filter [rad]. - REAL(4) :: Y_ErrLPFSlow ! Filtered yaw error by slow low pass filter [rad]. - REAL(4) :: Y_MErr ! Measured yaw error, measured + setpoint [rad] - END TYPE LocalVariables +TYPE, PUBLIC :: ControlParameters + REAL(4) :: CornerFreq ! Corner frequency (-3dB point) in the first-order low-pass filter, [rad/s] + INTEGER(4) :: LoggingLevel ! 0 = write no debug files, 1 = write standard output .dbg-file, 2 = write standard output .dbg-file and complete avrSWAP-array .dbg2-file + REAL(4) :: IPC_KI ! Integral gain for the individual pitch controller, [-]. 8E-10 + INTEGER(4) :: IPC_ControlMode ! Turn Individual Pitch Control (IPC) for fatigue load reductions (pitch contribution) on = 1/off = 0 + REAL(4) :: IPC_omegaHP ! High-pass filter cut-in frequency used to separate yaw-by-IPC contribution from blade load reduction contribution, [rad/s]. + REAL(4) :: IPC_omegaLP ! Low-pass filter corner frequency for the individual pitch controller, [rad/s]. + REAL(4) :: IPC_omegaNotch ! Notch filter corner frequency for the individual pitch controller, [rad/s]. + REAL(4) :: IPC_phi ! Phase offset added to the azimuth angle for the individual pitch controller, [rad]. + REAL(4) :: IPC_zetaHP ! High-pass filter damping value, [-]. + REAL(4) :: IPC_zetaLP ! Low-pass filter damping factor for the individual pitch controller, [-]. + REAL(4) :: IPC_zetaNotch ! Notch filter damping factor for the individual pitch controller, [-]. + INTEGER(4) :: PC_GS_n ! Amount of gain-scheduling table entries + REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_angles ! Gain-schedule table: pitch angles + REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_kp ! Gain-schedule table: pitch controller kp gains + REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_ki ! Gain-schedule table: pitch controller ki gains + REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_kd ! Gain-schedule table: pitch controller kd gains + REAL(4), DIMENSION(:), ALLOCATABLE :: PC_GS_tf ! Gain-schedule table: pitch controller tf gains (derivative filter) + REAL(4) :: PC_MaxPit ! Maximum physical pitch limit, [rad]. + REAL(4) :: PC_MinPit ! Minimum physical pitch limit, [rad]. + REAL(4) :: PC_MaxRat ! Maximum pitch rate (in absolute value) in pitch controller, [rad/s]. + REAL(4) :: PC_MinRat ! Minimum pitch rate (in absolute value) in pitch controller, [rad/s]. + REAL(4) :: PC_RefSpd ! Desired (reference) HSS speed for pitch controller, [rad/s]. + REAL(4) :: PC_SetPnt ! Record 5: Below-rated pitch angle set-point (deg) [used only with Bladed Interface] + REAL(4) :: PC_Switch ! Angle above lowest minimum pitch angle for switch [rad] + INTEGER(4) :: VS_ControlMode ! Generator torque control mode in above rated conditions, 0 = constant torque / 1 = constant power + REAL(4) :: VS_CtInSp ! Transitional generator speed (HSS side) between regions 1 and 1 1/2, [rad/s]. + REAL(4) :: VS_GenTrqArSatMax ! Above rated generator torque PI control saturation, [Nm] -- 212900 + REAL(4) :: VS_MaxOM ! Optimal mode maximum speed, [rad/s]. + REAL(4) :: VS_MaxRat ! Maximum torque rate (in absolute value) in torque controller, [Nm/s]. + REAL(4) :: VS_MaxTq ! Maximum generator torque in Region 3 (HSS side), [Nm]. -- chosen to be 10% above VS_RtTq + REAL(4) :: VS_MinTq ! Minimum generator (HSS side), [Nm]. + REAL(4) :: VS_MinOM ! Optimal mode minimum speed, [rad/s] + REAL(4) :: VS_Rgn2K ! Generator torque constant in Region 2 (HSS side), N-m/(rad/s)^2 + REAL(4) :: VS_RtPwr ! Wind turbine rated power [W] + REAL(4) :: VS_RtTq ! Rated torque, [Nm]. + REAL(4) :: VS_RtSpd ! Rated generator speed [rad/s] + INTEGER(4) :: VS_n ! Number of controller gains + REAL(4), DIMENSION(:), ALLOCATABLE :: VS_KP ! Proportional gain for generator PI torque controller, used in the transitional 2.5 region + REAL(4), DIMENSION(:), ALLOCATABLE :: VS_KI ! Integral gain for generator PI torque controller, used in the transitional 2.5 region + INTEGER(4) :: Y_ControlMode ! Yaw control mode: (0 = no yaw control, 1 = yaw rate control, 2 = yaw-by-IPC) + REAL(4) :: Y_ErrThresh ! Error threshold [rad]. Turbine begins to yaw when it passes this. (104.71975512) -- 1.745329252 + INTEGER(4) :: Y_IPC_n ! Number of controller gains (yaw-by-IPC) + REAL(4), DIMENSION(:), ALLOCATABLE :: Y_IPC_KP ! Yaw-by-IPC proportional controller gain Kp + REAL(4), DIMENSION(:), ALLOCATABLE :: Y_IPC_KI ! Yaw-by-IPC integral controller gain Ki + REAL(4) :: Y_MErrSet ! Yaw alignment error, setpoint [rad] + REAL(4) :: Y_omegaLPFast ! Corner frequency fast low pass filter, 1.0 [Hz] + REAL(4) :: Y_omegaLPSlow ! Corner frequency slow low pass filter, 1/60 [Hz] + REAL(4) :: Y_Rate ! Yaw rate [rad/s] - TYPE, PUBLIC :: ObjectInstances - INTEGER(4) :: instLPF - INTEGER(4) :: instSecLPF - INTEGER(4) :: instHPF - INTEGER(4) :: instNotchSlopes - INTEGER(4) :: instNotch - END TYPE ObjectInstances + REAL(4) :: PC_RtTq99 ! 99% of the rated torque value, using for switching between pitch and torque control, [Nm]. + REAL(4) :: VS_Rgn2MaxTq ! Maximum torque at the end of the below-rated region 2, [Nm] + REAL(4) :: VS_Rgn2MinTq ! Minimum torque at the beginning of the below-rated region 2, [Nm] + REAL(4) :: VS_Rgn3MP ! Minimum pitch angle at which the torque is computed as if we are in region 3 regardless of the generator speed, [rad]. +END TYPE ControlParameters + +TYPE, PUBLIC :: LocalVariables + ! From avrSWAP + INTEGER(4) :: iStatus + REAL(4) :: Time + REAL(4) :: DT + REAL(4) :: VS_GenPwr + REAL(4) :: GenSpeed + REAL(4) :: Y_M + REAL(4) :: HorWindV + REAL(4) :: rootMOOP(3) + REAL(4) :: BlPitch(3) + REAL(4) :: Azimuth + INTEGER(4) :: NumBl - !TYPE, PUBLIC :: PersistentVariables - !END TYPE PersistentVariables + ! Internal controller variables + REAL(4) :: GenSpeedF ! Filtered HSS (generator) speed [rad/s]. + REAL(4) :: GenTrq ! Electrical generator torque, [Nm]. + REAL(4) :: GenTrqAr ! Electrical generator torque, for above-rated PI-control [Nm]. + REAL(4) :: GenTrqBr ! Electrical generator torque, for below-rated PI-control [Nm]. + INTEGER(4) :: GlobalState ! Current global state to determine the behavior of the different controllers [-]. + REAL(4) :: IPC_PitComF(3) ! Commanded pitch of each blade as calculated by the individual pitch controller, F stands for low pass filtered, [rad]. + REAL(4) :: PC_KP ! Proportional gain for pitch controller at rated pitch (zero), [s]. + REAL(4) :: PC_KI ! Integral gain for pitch controller at rated pitch (zero), [-]. + REAL(4) :: PC_KD ! Differential gain for pitch controller at rated pitch (zero), [-]. + REAL(4) :: PC_TF ! First-order filter parameter for derivative action + REAL(4) :: PC_MaxPitVar ! Maximum pitch setting in pitch controller (variable) [rad]. + REAL(4) :: PC_PitComT ! Total command pitch based on the sum of the proportional and integral terms, [rad]. + REAL(4) :: PC_PitComT_IPC(3) ! Total command pitch based on the sum of the proportional and integral terms, including IPC term [rad]. + REAL(4) :: PC_PwrErr ! Power error with respect to rated power [W] + REAL(4) :: PC_SpdErr ! Current speed error (pitch control) [rad/s]. + REAL(4) :: PitCom(3) ! Commanded pitch of each blade the last time the controller was called, [rad]. + INTEGER(4) :: TestType ! Test variable, no use + REAL(4) :: VS_LastGenTrq ! Commanded electrical generator torque the last time the controller was called, [Nm]. + REAL(4) :: VS_SpdErrAr ! Current speed error (generator torque control) [rad/s]. + REAL(4) :: VS_SpdErrBr ! Current speed error (generator torque control) [rad/s]. + REAL(4) :: Y_AccErr ! Accumulated yaw error [rad]. + REAL(4) :: Y_ErrLPFFast ! Filtered yaw error by fast low pass filter [rad]. + REAL(4) :: Y_ErrLPFSlow ! Filtered yaw error by slow low pass filter [rad]. + REAL(4) :: Y_MErr ! Measured yaw error, measured + setpoint [rad]. + REAL(4) :: Y_YawEndT ! Yaw end time, [s]. Indicates the time up until which yaw is active with a fixed rate +END TYPE LocalVariables +TYPE, PUBLIC :: ObjectInstances + INTEGER(4) :: instLPF + INTEGER(4) :: instSecLPF + INTEGER(4) :: instHPF + INTEGER(4) :: instNotchSlopes + INTEGER(4) :: instNotch +END TYPE ObjectInstances END MODULE DRC_Types \ No newline at end of file diff --git a/Source/FunctionToolbox.f90 b/Source/FunctionToolbox.f90 index 0d845624..f2d75770 100644 --- a/Source/FunctionToolbox.f90 +++ b/Source/FunctionToolbox.f90 @@ -1,6 +1,7 @@ ! This module contains basic functions MODULE FunctionToolbox +USE Constants IMPLICIT NONE CONTAINS @@ -180,4 +181,141 @@ END FUNCTION DFController ! !END FUNCTION PRBSgen !------------------------------------------------------------------------------------------------------------------------------- -END MODULE FunctionToolbox + ! Stata machine, determines the state of the wind turbine to determine the corresponding control actions + ! States: + ! - 1, idling, wind ad rotor speed too low for start-up: set pitch to vane position and torque to minimum + ! - 2, start-up mode, set pitch demand to start-up pitch angle for maximum aerodynamic torque and torque demand to minimum + ! - 3, start-up2normal + ! - 4, Region 1.5 operation, torque control to keep the rotor at cut-in speed towards the Cp-max operational curve + ! - 5, Region 2, operation, maximum rotor power efficiency (Cp-max) tracking, keep TSR constant at a fixed fine-pitch angle + ! - 6, Region 2.5, transition between below and above-rated operating conditions (near-rated region) using PI torque control + ! - 7, Region 3, above-rated operation using pitch control + !REAL FUNCTION StateMachine(LocalVar, CntrPar) + ! + ! USE DRC_Types, ONLY : LocalVariables, ControlParameters + ! + ! IMPLICIT NONE + ! + ! ! Inputs + ! TYPE(ControlParameters), INTENT(IN) :: CntrPar + ! + ! ! Inputs/outputs + ! TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + ! + ! ! Local + ! + ! + !END FUNCTION StateMachine + !------------------------------------------------------------------------------------------------------------------------------- + SUBROUTINE Debug(LocalVar, CntrPar, avrSWAP, RootName, size_avcOUTNAME) + USE, INTRINSIC :: ISO_C_Binding + USE DRC_Types, ONLY : LocalVariables, ControlParameters + + IMPLICIT NONE + + TYPE(ControlParameters), INTENT(IN) :: CntrPar + TYPE(LocalVariables), INTENT(IN) :: LocalVar + + INTEGER(4), INTENT(IN) :: size_avcOUTNAME + INTEGER(4) :: I ! Generic index. + CHARACTER(1), PARAMETER :: Tab = CHAR(9) ! The tab character. + CHARACTER(25), PARAMETER :: FmtDat = "(F8.3,99('"//Tab//"',ES10.3E2,:)) " ! The format of the debugging data + INTEGER(4), PARAMETER :: UnDb = 85 ! I/O unit for the debugging information + INTEGER(4), PARAMETER :: UnDb2 = 86 ! I/O unit for the debugging information, avrSWAP + REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. + CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName ! a Fortran version of the input C string (not considered an array here) [subtract 1 for the C null-character] + + !.............................................................................................................................. + ! Initializing debug file + !.............................................................................................................................. + IF (LocalVar%iStatus == 0) THEN ! .TRUE. if we're on the first call to the DLL + ! If we're debugging, open the debug file and write the header: + IF (CntrPar%LoggingLevel > 0) THEN + OPEN (UnDb, FILE=TRIM(RootName)//'.dbg', STATUS='REPLACE') + WRITE (UnDb,'(A)') ' LocalVar%Time ' //Tab//'LocalVar%PC_PitComT ' //Tab//'LocalVar%PC_SpdErr ' //Tab//'LocalVar%PC_KP ' //Tab//'LocalVar%PC_KI ' //Tab//'LocalVar%Y_M ' //Tab//'LocalVar%rootMOOP(1) '//Tab//'VS_RtPwr '//Tab//'LocalVar%GenTrq' + WRITE (UnDb,'(A)') ' (sec) ' //Tab//'(rad) ' //Tab//'(rad/s) '//Tab//'(-) ' //Tab//'(-) ' //Tab//'(rad) ' //Tab//'(?) ' //Tab//'(W) '//Tab//'(Nm) ' + END IF + + IF (CntrPar%LoggingLevel > 1) THEN + OPEN(UnDb2, FILE=TRIM(RootName)//'.dbg2', STATUS='REPLACE') + WRITE(UnDb2,'(/////)') + WRITE(UnDb2,'(A,85("'//Tab//'AvrSWAP(",I2,")"))') 'LocalVar%Time ', (i,i=1,85) + WRITE(UnDb2,'(A,85("'//Tab//'(-)"))') '(s)' + END IF + ELSE + !.............................................................................................................................. + ! Output debugging information if requested: + IF (CntrPar%LoggingLevel > 0) THEN + WRITE (UnDb,FmtDat) LocalVar%Time, LocalVar%PC_PitComT, LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, LocalVar%Y_MErr, LocalVar%rootMOOP(1), CntrPar%VS_RtPwr, LocalVar%GenTrq + END IF + + IF (CntrPar%LoggingLevel > 1) THEN + WRITE (UnDb2,FmtDat) LocalVar%Time, avrSWAP(1:85) + END IF + END IF + + IF (MODULO(LocalVar%Time, 10.0) == 0.0) THEN + !LocalVar%TestType = LocalVar%TestType + 10 + !PRINT *, LocalVar%TestType + END IF + END SUBROUTINE Debug + !------------------------------------------------------------------------------------------------------------------------------- + !The Coleman or d-q axis transformation transforms the root out of plane bending moments of each turbine blade + !to a direct axis and a quadrature axis + SUBROUTINE ColemanTransform(rootMOOP, aziAngle, axisTilt, axisYaw) + !............................................................................................................................... + + IMPLICIT NONE + + ! Inputs + + REAL(4), INTENT(IN) :: rootMOOP(3) ! Root out of plane bending moments of each blade + REAL(4), INTENT(IN) :: aziAngle ! Rotor azimuth angle + + ! Outputs + + REAL(4), INTENT(OUT) :: axisTilt, axisYaw ! Direct axis and quadrature axis outputted by this transform + + ! Local + + REAL(4), PARAMETER :: phi2 = 2.0/3.0*PI ! Phase difference from first to second blade + REAL(4), PARAMETER :: phi3 = 4.0/3.0*PI ! Phase difference from first to third blade + + ! Body + + axisTilt = 2.0/3.0 * (cos(aziAngle)*rootMOOP(1) + cos(aziAngle+phi2)*rootMOOP(2) + cos(aziAngle+phi3)*rootMOOP(3)) + axisYaw = 2.0/3.0 * (sin(aziAngle)*rootMOOP(1) + sin(aziAngle+phi2)*rootMOOP(2) + sin(aziAngle+phi3)*rootMOOP(3)) + + END SUBROUTINE ColemanTransform + !------------------------------------------------------------------------------------------------------------------------------- + !The inverse Coleman or d-q axis transformation transforms the direct axis and quadrature axis + !back to root out of plane bending moments of each turbine blade + SUBROUTINE ColemanTransformInverse(axisTilt, axisYaw, aziAngle, phi, PitComIPC) + !............................................................................................................................... + + IMPLICIT NONE + + ! Inputs + + REAL(4), INTENT(IN) :: axisTilt, axisYaw ! Direct axis and quadrature axis + REAL(4), INTENT(IN) :: aziAngle ! Rotor azimuth angle + REAL(4), INTENT(IN) :: phi ! Phase shift added to the azimuth angle + + ! Outputs + + REAL(4), INTENT(OUT) :: PitComIPC (3) ! Root out of plane bending moments of each blade + + ! Local + + REAL(4), PARAMETER :: phi2 = 2.0/3.0*PI ! Phase difference from first to second blade + REAL(4), PARAMETER :: phi3 = 4.0/3.0*PI ! Phase difference from first to third blade + + ! Body + + PitComIPC(1) = cos(aziAngle+phi)*axisTilt + sin(aziAngle+phi)*axisYaw + PitComIPC(2) = cos(aziAngle+phi+phi2)*axisTilt + sin(aziAngle+phi+phi2)*axisYaw + PitComIPC(3) = cos(aziAngle+phi+phi3)*axisTilt + sin(aziAngle+phi+phi3)*axisYaw + + END SUBROUTINE ColemanTransformInverse + !------------------------------------------------------------------------------------------------------------------------------- +END MODULE FunctionToolbox \ No newline at end of file diff --git a/Source/IPC.f90 b/Source/IPC.f90 deleted file mode 100644 index b0cb5aa0..00000000 --- a/Source/IPC.f90 +++ /dev/null @@ -1,183 +0,0 @@ -!------------------------------------------------------------------------------------------------------------------------------- -! Individual pitch control subroutine -!SUBROUTINE IPC(rootMOOP, aziAngle, phi, Y_MErr, DT, KInter, Y_IPC_KP, Y_IPC_KI, omegaHP, omegaLP, omegaNotch, zetaHP, zetaLP, zetaNotch, iStatus, IPC_ControlMode, Y_ControlMode, NumBl, PitComIPCF, objInst) -SUBROUTINE IPC(LocalVar, CntrPar, objInst) -!............................................................................................................................... - - USE :: FunctionToolbox - USE :: Filters - USE DRC_Types, ONLY : LocalVariables, ControlParameters, ObjectInstances - - IMPLICIT NONE - - !------------------------------------------------------------------------------------------------------------------------------ - ! Variable declaration and initialization - !------------------------------------------------------------------------------------------------------------------------------ - - ! Inputs - - !REAL(4), INTENT(IN) :: LocalVar%Azimuth ! Rotor azimuth angle - !REAL(4), INTENT(IN) :: LocalVar%DT ! Time step - !REAL(4), INTENT(IN) :: CntrPar%IPC_KI ! Gain for the integrator - !REAL(4), INTENT(IN) :: CntrPar%IPC_omegaHP ! High-pass filter cut-in frequency - !REAL(4), INTENT(IN) :: CntrPar%IPC_omegaLP ! Low-pass filter cut-off frequency - !REAL(4), INTENT(IN) :: CntrPar%IPC_omegaNotch ! Notch filter frequency - !REAL(4), INTENT(IN) :: CntrPar%IPC_phi ! Phase offset added to the azimuth angle - !REAL(4), INTENT(IN) :: LocalVariables%rootMOOP(3) ! Root out of plane bending moments of each blade - !REAL(4), INTENT(IN) :: LocalVariables%Y_MErr ! Yaw alignment error, measured [rad] - !REAL(4), INTENT(IN) :: CntrPar%IPC_zetaHP ! High-pass filter damping value - !REAL(4), INTENT(IN) :: CntrPar%IPC_zetaLP ! Low-pass filter damping value - !REAL(4), INTENT(IN) :: CntrPar%IPC_zetaNotch ! Notch filter damping value - !INTEGER(4), INTENT(IN) :: LocalVariables%iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. - !INTEGER(4), INTENT(IN) :: LocalVariables%NumBl ! Number of turbine blades - !INTEGER(4), INTENT(IN) :: CntrPar%Y_ControlMode ! Yaw control mode - !REAL(4), INTENT(IN) :: CntrPar%Y_IPC_KP ! Yaw-by-IPC proportional controller gain Kp - !REAL(4), INTENT(IN) :: CntrPar%Y_IPC_KI ! Yaw-by-IPC integral controller gain Ki - !INTEGER(4), INTENT(IN) :: CntrPar%IPC_ControlMode ! Turn Individual Pitch Control (IPC) for fatigue load reductions (pitch contribution) on = 1/off = 0 - - ! Outputs - - !REAL(4), INTENT(OUT) :: LocalVar%IPC_PitComF(3) ! Filtered pitch angle of each rotor blade - - ! Inputs/outputs - - TYPE(LocalVariables), INTENT(INOUT) :: LocalVar - TYPE(ControlParameters), INTENT(IN) :: CntrPar - TYPE(ObjectInstances), INTENT(INOUT) :: objInst - - ! Local variables - - REAL(4), PARAMETER :: PI = 3.14159265359 ! Mathematical constant pi - REAL(4) :: PitComIPC(3) - INTEGER(4) :: K ! Integer used to loop through turbine blades - REAL(4) :: axisTilt, axisYaw, axisYawF ! Direct axis and quadrature axis outputted by Coleman transform - REAL(4), SAVE :: IntAxisTilt, IntAxisYaw ! Integral of the direct axis and quadrature axis - REAL(4) :: IntAxisYawIPC ! IPC contribution with yaw-by-IPC component - REAL(4) :: Y_MErrF, Y_MErrF_IPC ! Unfiltered and filtered yaw alignment error [rad] - REAL(4) :: PitComIPC_woYaw(3) - - !------------------------------------------------------------------------------------------------------------------------------ - ! Body - !------------------------------------------------------------------------------------------------------------------------------ - ! Calculates the commanded pitch angles. - ! NOTE: if it is required for this subroutine to be used multiple times (for 1p and 2p IPC for example), the saved variables - ! IntAxisTilt and IntAxisYaw need to be modified so that they support multiple instances (see LPFilter in the Filters module). - !------------------------------------------------------------------------------------------------------------------------------ - ! Filter rootMOOPs with notch filter - - !DO K = 1,LocalVar%NumBl - ! Instances 1-3 of the Notch Filter are reserved for this routine. - ! rootMOOPF(K) = LocalVar%rootMOOP(K) ! Notch filter currently not in use - !END DO - - ! Initialization - ! Set integrals to be 0 in the first time step - - IF(LocalVar%iStatus==0) THEN - IntAxisTilt = 0.0 - IntAxisYaw = 0.0 - END IF - - ! Body - ! Pass rootMOOPs through the Coleman transform to get the direct and quadrature axis - - CALL ColemanTransform(LocalVar%rootMOOP, LocalVar%Azimuth, axisTilt, axisYaw) - - ! High-pass filter the MBC yaw component and filter yaw alignment error, and compute the yaw-by-IPC contribution - - IF (CntrPar%Y_ControlMode == 2) THEN - axisYawF = HPFilter(axisYaw, LocalVar%DT, CntrPar%IPC_omegaHP, LocalVar%iStatus, .FALSE., objInst%instHPF) - Y_MErrF = SecLPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%IPC_omegaLP, CntrPar%IPC_zetaLP, LocalVar%iStatus, .FALSE., objInst%instSecLPF) - Y_MErrF_IPC = PIController(Y_MErrF, CntrPar%Y_IPC_KP(1), CntrPar%Y_IPC_KI(1), -100.0, 100.0, LocalVar%DT, 0.0, .FALSE., 3) - ELSE - axisYawF = axisYaw - Y_MErrF = 0.0 - END IF - - ! Integrate the signal and multiply with the IPC gain - IF (CntrPar%IPC_ControlMode == 1) THEN - IntAxisTilt = IntAxisTilt + LocalVar%DT * CntrPar%IPC_KI * axisTilt - IntAxisYaw = IntAxisYaw + LocalVar%DT * CntrPar%IPC_KI * axisYawF - ELSE - IntAxisTilt = 0.0 - IntAxisYaw = 0.0 - END IF - - ! Add the yaw-by-IPC contribution - - IntAxisYawIPC = IntAxisYaw + Y_MErrF_IPC - - ! Pass direct and quadrature axis through the inverse Coleman transform to get the commanded pitch angles - - CALL ColemanTransformInverse(IntAxisTilt, IntAxisYawIPC, LocalVar%Azimuth, CntrPar%IPC_phi, PitComIPC) - - ! Filter PitComIPC with second order low pass filter - - DO K = 1,LocalVar%NumBl - ! Instances 1-3 of the Second order Low-Pass Filter are reserved for this routine. - ! LocalVar%IPC_PitComF(K) = SecLPFilter(PitComIPC(K), LocalVar%DT, CntrPar%IPC_omegaLP, CntrPar%IPC_zetaLP, LocalVar%iStatus, K) - LocalVar%IPC_PitComF(K) = PitComIPC(K) - END DO - -CONTAINS - - !------------------------------------------------------------------------------------------------------------------------------- - !The Coleman or d-q axis transformation transforms the root out of plane bending moments of each turbine blade - !to a direct axis and a quadrature axis - SUBROUTINE ColemanTransform(rootMOOP, aziAngle, axisTilt, axisYaw) - !............................................................................................................................... - - IMPLICIT NONE - - ! Inputs - - REAL(4), INTENT(IN) :: rootMOOP(3) ! Root out of plane bending moments of each blade - REAL(4), INTENT(IN) :: aziAngle ! Rotor azimuth angle - - ! Outputs - - REAL(4), INTENT(OUT) :: axisTilt, axisYaw ! Direct axis and quadrature axis outputted by this transform - - ! Local - - REAL(4), PARAMETER :: phi2 = 2.0/3.0*PI ! Phase difference from first to second blade - REAL(4), PARAMETER :: phi3 = 4.0/3.0*PI ! Phase difference from first to third blade - - ! Body - - axisTilt = 2.0/3.0 * (cos(aziAngle)*rootMOOP(1) + cos(aziAngle+phi2)*rootMOOP(2) + cos(aziAngle+phi3)*rootMOOP(3)) - axisYaw = 2.0/3.0 * (sin(aziAngle)*rootMOOP(1) + sin(aziAngle+phi2)*rootMOOP(2) + sin(aziAngle+phi3)*rootMOOP(3)) - - END SUBROUTINE ColemanTransform - !------------------------------------------------------------------------------------------------------------------------------- - !The inverse Coleman or d-q axis transformation transforms the direct axis and quadrature axis - !back to root out of plane bending moments of each turbine blade - SUBROUTINE ColemanTransformInverse(axisTilt, axisYaw, aziAngle, phi, PitComIPC) - !............................................................................................................................... - - IMPLICIT NONE - - ! Inputs - - REAL(4), INTENT(IN) :: axisTilt, axisYaw ! Direct axis and quadrature axis - REAL(4), INTENT(IN) :: aziAngle ! Rotor azimuth angle - REAL(4), INTENT(IN) :: phi ! Phase shift added to the azimuth angle - - ! Outputs - - REAL(4), INTENT(OUT) :: PitComIPC (3) ! Root out of plane bending moments of each blade - - ! Local - - REAL(4), PARAMETER :: phi2 = 2.0/3.0*PI ! Phase difference from first to second blade - REAL(4), PARAMETER :: phi3 = 4.0/3.0*PI ! Phase difference from first to third blade - - ! Body - - PitComIPC(1) = cos(aziAngle+phi)*axisTilt + sin(aziAngle+phi)*axisYaw - PitComIPC(2) = cos(aziAngle+phi+phi2)*axisTilt + sin(aziAngle+phi+phi2)*axisYaw - PitComIPC(3) = cos(aziAngle+phi+phi3)*axisTilt + sin(aziAngle+phi+phi3)*axisYaw - - END SUBROUTINE ColemanTransformInverse - !------------------------------------------------------------------------------------------------------------------------------- -END SUBROUTINE IPC diff --git a/Source/Obj_win32/filters.mod b/Source/Obj_win32/filters.mod deleted file mode 100644 index 3b84261c..00000000 Binary files a/Source/Obj_win32/filters.mod and /dev/null differ diff --git a/Source/Obj_win32/functiontoolbox.mod b/Source/Obj_win32/functiontoolbox.mod deleted file mode 100644 index ae013835..00000000 Binary files a/Source/Obj_win32/functiontoolbox.mod and /dev/null differ diff --git a/Source/ReadParameters.f90 b/Source/ReadParameters.f90 deleted file mode 100644 index abb579d5..00000000 --- a/Source/ReadParameters.f90 +++ /dev/null @@ -1,127 +0,0 @@ -MODULE ReadParameters - - USE DRC_Types - IMPLICIT NONE - - TYPE(ControlParameters), SAVE :: CntrPar - TYPE(LocalVariables) :: LocalVar - -CONTAINS - !.............................................................................................................................. - ! Read all constant control parameters from ControllerParameters.in parameter file - !.............................................................................................................................. - SUBROUTINE ReadControlParameterFileSub() - - INTEGER(4), PARAMETER :: UnControllerParameters = 89 - OPEN(unit=UnControllerParameters, file='ControllerParameters.in', status='old', action='read') - - !------------------- GENERAL CONSTANTS ------------------- - READ(UnControllerParameters, *) CntrPar%CornerFreq - READ(UnControllerParameters, *) CntrPar%LoggingLevel - - !------------------- IPC CONSTANTS ----------------------- - READ(UnControllerParameters, *) CntrPar%IPC_KI - READ(UnControllerParameters, *) CntrPar%IPC_ControlMode - READ(UnControllerParameters, *) CntrPar%IPC_omegaHP - READ(UnControllerParameters, *) CntrPar%IPC_omegaLP - READ(UnControllerParameters, *) CntrPar%IPC_omegaNotch - READ(UnControllerParameters, *) CntrPar%IPC_phi - READ(UnControllerParameters, *) CntrPar%IPC_zetaHP - READ(UnControllerParameters, *) CntrPar%IPC_zetaLP - READ(UnControllerParameters, *) CntrPar%IPC_zetaNotch - - !------------------- PITCH CONSTANTS ----------------------- - READ(UnControllerParameters, *) CntrPar%PC_GS_n - - ALLOCATE(CntrPar%PC_GS_angles(CntrPar%PC_GS_n)) - READ(UnControllerParameters,*) CntrPar%PC_GS_angles - - ALLOCATE(CntrPar%PC_GS_kp(CntrPar%PC_GS_n)) - READ(UnControllerParameters,*) CntrPar%PC_GS_kp - - ALLOCATE(CntrPar%PC_GS_ki(CntrPar%PC_GS_n)) - READ(UnControllerParameters,*) CntrPar%PC_GS_ki - - ALLOCATE(CntrPar%PC_GS_kd(CntrPar%PC_GS_n)) - READ(UnControllerParameters,*) CntrPar%PC_GS_kd - - ALLOCATE(CntrPar%PC_GS_tf(CntrPar%PC_GS_n)) - READ(UnControllerParameters,*) CntrPar%PC_GS_tf - - READ(UnControllerParameters, *) CntrPar%PC_MaxPit - READ(UnControllerParameters, *) CntrPar%PC_MinPit - READ(UnControllerParameters, *) CntrPar%PC_MaxRat - READ(UnControllerParameters, *) CntrPar%PC_MinRat - READ(UnControllerParameters, *) CntrPar%PC_RefSpd - READ(UnControllerParameters, *) CntrPar%PC_SetPnt - READ(UnControllerParameters, *) CntrPar%PC_Switch - - !------------------- TORQUE CONSTANTS ----------------------- - READ(UnControllerParameters, *) CntrPar%VS_ControlMode - READ(UnControllerParameters, *) CntrPar%VS_CtInSp - READ(UnControllerParameters, *) CntrPar%VS_GenTrqArSatMax - READ(UnControllerParameters, *) CntrPar%VS_MaxOM - READ(UnControllerParameters, *) CntrPar%VS_MaxRat - READ(UnControllerParameters, *) CntrPar%VS_MaxTq - READ(UnControllerParameters, *) CntrPar%VS_MinTq - READ(UnControllerParameters, *) CntrPar%VS_MinOM - READ(UnControllerParameters, *) CntrPar%VS_Rgn2K - READ(UnControllerParameters, *) CntrPar%VS_RtPwr - READ(UnControllerParameters, *) CntrPar%VS_RtTq - READ(UnControllerParameters, *) CntrPar%VS_RtSpd - READ(UnControllerParameters, *) CntrPar%VS_n - - ALLOCATE(CntrPar%VS_KP(CntrPar%VS_n)) - READ(UnControllerParameters,*) CntrPar%VS_KP - - ALLOCATE(CntrPar%VS_KI(CntrPar%VS_n)) - READ(UnControllerParameters,*) CntrPar%VS_KI - - !------------------- YAW CONSTANTS ----------------------- - READ(UnControllerParameters, *) CntrPar%Y_ControlMode - READ(UnControllerParameters, *) CntrPar%Y_ErrThresh - READ(UnControllerParameters, *) CntrPar%Y_IPC_n - - ALLOCATE(CntrPar%Y_IPC_KP(CntrPar%Y_IPC_n)) - READ(UnControllerParameters,*) CntrPar%Y_IPC_KP - - ALLOCATE(CntrPar%Y_IPC_KI(CntrPar%Y_IPC_n)) - READ(UnControllerParameters,*) CntrPar%Y_IPC_KI - - READ(UnControllerParameters, *) CntrPar%Y_MErrSet - READ(UnControllerParameters, *) CntrPar%Y_omegaLPFast - READ(UnControllerParameters, *) CntrPar%Y_omegaLPSlow - READ(UnControllerParameters, *) CntrPar%Y_Rate - - !------------------- CALCULATED CONSTANTS ----------------------- - CntrPar%PC_RtTq99 = CntrPar%VS_RtTq*0.99 - CntrPar%VS_Rgn2MinTq = CntrPar%VS_Rgn2K*CntrPar%VS_MinOM**2 - CntrPar%VS_Rgn2MaxTq = CntrPar%VS_Rgn2K*CntrPar%VS_MaxOM**2 - CntrPar%VS_Rgn3MP = CntrPar%PC_SetPnt + CntrPar%PC_Switch - - CLOSE(UnControllerParameters) - END SUBROUTINE ReadControlParameterFileSub - - SUBROUTINE ReadAvrSWAP(avrSWAP) - - USE, INTRINSIC :: ISO_C_Binding - REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. - - ! Load variables from calling program (See Appendix A of Bladed User's Guide): - LocalVar%iStatus = NINT(avrSWAP(1)) - LocalVar%Time = avrSWAP(2) - LocalVar%DT = avrSWAP(3) - LocalVar%BlPitch(1) = avrSWAP(4) - LocalVar%VS_GenPwr = avrSWAP(15) - LocalVar%GenSpeed = avrSWAP(20) - LocalVar%Y_M = avrSWAP(24) - LocalVar%HorWindV = avrSWAP(27) - LocalVar%rootMOOP(1) = avrSWAP(30) - LocalVar%rootMOOP(2) = avrSWAP(31) - LocalVar%rootMOOP(3) = avrSWAP(32) - LocalVar%BlPitch(2) = avrSWAP(33) - LocalVar%BlPitch(3) = avrSWAP(34) - LocalVar%Azimuth = avrSWAP(60) - LocalVar%NumBl = NINT(avrSWAP(61)) - END SUBROUTINE ReadAvrSWAP -END MODULE ReadParameters \ No newline at end of file diff --git a/Source/ReadSetParameters.f90 b/Source/ReadSetParameters.f90 new file mode 100644 index 00000000..fed826af --- /dev/null +++ b/Source/ReadSetParameters.f90 @@ -0,0 +1,340 @@ +MODULE ReadSetParameters + + USE, INTRINSIC :: ISO_C_Binding + IMPLICIT NONE + +CONTAINS + !.............................................................................................................................. + ! Read all constant control parameters from ControllerParameters.in parameter file + !.............................................................................................................................. + SUBROUTINE ReadControlParameterFileSub(CntrPar) + USE DRC_Types, ONLY : ControlParameters + + INTEGER(4), PARAMETER :: UnControllerParameters = 89 + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + + OPEN(unit=UnControllerParameters, file='ControllerParameters.in', status='old', action='read') + + !------------------- GENERAL CONSTANTS ------------------- + READ(UnControllerParameters, *) CntrPar%CornerFreq + READ(UnControllerParameters, *) CntrPar%LoggingLevel + + !------------------- IPC CONSTANTS ----------------------- + READ(UnControllerParameters, *) CntrPar%IPC_KI + READ(UnControllerParameters, *) CntrPar%IPC_ControlMode + READ(UnControllerParameters, *) CntrPar%IPC_omegaHP + READ(UnControllerParameters, *) CntrPar%IPC_omegaLP + READ(UnControllerParameters, *) CntrPar%IPC_omegaNotch + READ(UnControllerParameters, *) CntrPar%IPC_phi + READ(UnControllerParameters, *) CntrPar%IPC_zetaHP + READ(UnControllerParameters, *) CntrPar%IPC_zetaLP + READ(UnControllerParameters, *) CntrPar%IPC_zetaNotch + + !------------------- PITCH CONSTANTS ----------------------- + READ(UnControllerParameters, *) CntrPar%PC_GS_n + + ALLOCATE(CntrPar%PC_GS_angles(CntrPar%PC_GS_n)) + READ(UnControllerParameters,*) CntrPar%PC_GS_angles + + ALLOCATE(CntrPar%PC_GS_kp(CntrPar%PC_GS_n)) + READ(UnControllerParameters,*) CntrPar%PC_GS_kp + + ALLOCATE(CntrPar%PC_GS_ki(CntrPar%PC_GS_n)) + READ(UnControllerParameters,*) CntrPar%PC_GS_ki + + ALLOCATE(CntrPar%PC_GS_kd(CntrPar%PC_GS_n)) + READ(UnControllerParameters,*) CntrPar%PC_GS_kd + + ALLOCATE(CntrPar%PC_GS_tf(CntrPar%PC_GS_n)) + READ(UnControllerParameters,*) CntrPar%PC_GS_tf + + READ(UnControllerParameters, *) CntrPar%PC_MaxPit + READ(UnControllerParameters, *) CntrPar%PC_MinPit + READ(UnControllerParameters, *) CntrPar%PC_MaxRat + READ(UnControllerParameters, *) CntrPar%PC_MinRat + READ(UnControllerParameters, *) CntrPar%PC_RefSpd + READ(UnControllerParameters, *) CntrPar%PC_SetPnt + READ(UnControllerParameters, *) CntrPar%PC_Switch + + !------------------- TORQUE CONSTANTS ----------------------- + READ(UnControllerParameters, *) CntrPar%VS_ControlMode + READ(UnControllerParameters, *) CntrPar%VS_CtInSp + READ(UnControllerParameters, *) CntrPar%VS_GenTrqArSatMax + READ(UnControllerParameters, *) CntrPar%VS_MaxOM + READ(UnControllerParameters, *) CntrPar%VS_MaxRat + READ(UnControllerParameters, *) CntrPar%VS_MaxTq + READ(UnControllerParameters, *) CntrPar%VS_MinTq + READ(UnControllerParameters, *) CntrPar%VS_MinOM + READ(UnControllerParameters, *) CntrPar%VS_Rgn2K + READ(UnControllerParameters, *) CntrPar%VS_RtPwr + READ(UnControllerParameters, *) CntrPar%VS_RtTq + READ(UnControllerParameters, *) CntrPar%VS_RtSpd + READ(UnControllerParameters, *) CntrPar%VS_n + + ALLOCATE(CntrPar%VS_KP(CntrPar%VS_n)) + READ(UnControllerParameters,*) CntrPar%VS_KP + + ALLOCATE(CntrPar%VS_KI(CntrPar%VS_n)) + READ(UnControllerParameters,*) CntrPar%VS_KI + + !------------------- YAW CONSTANTS ----------------------- + READ(UnControllerParameters, *) CntrPar%Y_ControlMode + READ(UnControllerParameters, *) CntrPar%Y_ErrThresh + READ(UnControllerParameters, *) CntrPar%Y_IPC_n + + ALLOCATE(CntrPar%Y_IPC_KP(CntrPar%Y_IPC_n)) + READ(UnControllerParameters,*) CntrPar%Y_IPC_KP + + ALLOCATE(CntrPar%Y_IPC_KI(CntrPar%Y_IPC_n)) + READ(UnControllerParameters,*) CntrPar%Y_IPC_KI + + READ(UnControllerParameters, *) CntrPar%Y_MErrSet + READ(UnControllerParameters, *) CntrPar%Y_omegaLPFast + READ(UnControllerParameters, *) CntrPar%Y_omegaLPSlow + READ(UnControllerParameters, *) CntrPar%Y_Rate + + !------------------- CALCULATED CONSTANTS ----------------------- + CntrPar%PC_RtTq99 = CntrPar%VS_RtTq*0.99 + CntrPar%VS_Rgn2MinTq = CntrPar%VS_Rgn2K*CntrPar%VS_MinOM**2 + CntrPar%VS_Rgn2MaxTq = CntrPar%VS_Rgn2K*CntrPar%VS_MaxOM**2 + CntrPar%VS_Rgn3MP = CntrPar%PC_SetPnt + CntrPar%PC_Switch + + CLOSE(UnControllerParameters) + END SUBROUTINE ReadControlParameterFileSub + + SUBROUTINE ReadAvrSWAP(avrSWAP, LocalVar) + USE DRC_Types, ONLY : LocalVariables + + REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + + ! Load variables from calling program (See Appendix A of Bladed User's Guide): + LocalVar%iStatus = NINT(avrSWAP(1)) + LocalVar%Time = avrSWAP(2) + LocalVar%DT = avrSWAP(3) + LocalVar%BlPitch(1) = avrSWAP(4) + LocalVar%VS_GenPwr = avrSWAP(15) + LocalVar%GenSpeed = avrSWAP(20) + LocalVar%Y_M = avrSWAP(24) + LocalVar%HorWindV = avrSWAP(27) + LocalVar%rootMOOP(1) = avrSWAP(30) + LocalVar%rootMOOP(2) = avrSWAP(31) + LocalVar%rootMOOP(3) = avrSWAP(32) + LocalVar%BlPitch(2) = avrSWAP(33) + LocalVar%BlPitch(3) = avrSWAP(34) + LocalVar%Azimuth = avrSWAP(60) + LocalVar%NumBl = NINT(avrSWAP(61)) + END SUBROUTINE ReadAvrSWAP + + SUBROUTINE Assert(LocalVar, CntrPar, avrSWAP, aviFAIL, ErrMsg, size_avcMSG) + + USE, INTRINSIC :: ISO_C_Binding + USE DRC_Types, ONLY : LocalVariables, ControlParameters + + IMPLICIT NONE + + ! Inputs + TYPE(ControlParameters), INTENT(IN) :: CntrPar + TYPE(LocalVariables), INTENT(IN) :: LocalVar + INTEGER(4), INTENT(IN) :: size_avcMSG + REAL(C_FLOAT), INTENT(IN) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. + + ! Outputs + INTEGER(C_INT), INTENT(OUT) :: aviFAIL ! A flag used to indicate the success of this DLL call set as follows: 0 if the DLL call was successful, >0 if the DLL call was successful but cMessage should be issued as a warning messsage, <0 if the DLL call was unsuccessful or for any other reason the simulation is to be stopped at this point with cMessage as the error message. + CHARACTER(size_avcMSG-1), INTENT(OUT) :: ErrMsg ! a Fortran version of the C string argument (not considered an array here) [subtract 1 for the C null-character] + + ! Local + + !.............................................................................................................................. + ! Check validity of input parameters: + !.............................................................................................................................. + + IF (CntrPar%CornerFreq <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'CornerFreq must be greater than zero.' + ENDIF + + IF (LocalVar%DT <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'DT must be greater than zero.' + ENDIF + + IF (CntrPar%VS_CtInSp < 0.0) THEN + aviFAIL = -1 + ErrMsg = 'VS_CtInSp must not be negative.' + ENDIF + + IF (CntrPar%VS_MinOM <= CntrPar%VS_CtInSp) THEN + aviFAIL = -1 + ErrMsg = 'VS_MinOM must be greater than VS_CtInSp.' + ENDIF + + IF (CntrPar%VS_MaxRat <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'VS_MaxRat must be greater than zero.' + ENDIF + + IF (CntrPar%VS_RtTq < 0.0) THEN + aviFAIL = -1 + ErrMsg = 'VS_RtTw must not be negative.' + ENDIF + + IF (CntrPar%VS_Rgn2K < 0.0) THEN + aviFAIL = -1 + ErrMsg = 'VS_Rgn2K must not be negative.' + ENDIF + + IF (CntrPar%VS_MaxTq < CntrPar%VS_RtTq) THEN + aviFAIL = -1 + ErrMsg = 'VS_RtTq must not be greater than VS_MaxTq.' + ENDIF + + IF (CntrPar%VS_KP(1) > 0.0) THEN + aviFAIL = -1 + ErrMsg = 'VS_KP must be greater than zero.' + ENDIF + + IF (CntrPar%VS_KI(1) > 0.0) THEN + aviFAIL = -1 + ErrMsg = 'VS_KI must be greater than zero.' + ENDIF + + IF (CntrPar%PC_RefSpd <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'PC_RefSpd must be greater than zero.' + ENDIF + + IF (CntrPar%PC_MaxRat <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'PC_MaxRat must be greater than zero.' + ENDIF + + IF (CntrPar%PC_MinPit >= CntrPar%PC_MaxPit) THEN + aviFAIL = -1 + ErrMsg = 'PC_MinPit must be less than PC_MaxPit.' + ENDIF + + IF (CntrPar%IPC_KI <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'IPC_KI must be greater than zero.' + ENDIF + + IF (CntrPar%IPC_omegaLP <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'IPC_omegaLP must be greater than zero.' + ENDIF + + IF (CntrPar%IPC_omegaNotch <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'IPC_omegaNotch must be greater than zero.' + ENDIF + + IF (CntrPar%IPC_phi <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'IPC_phi must be greater than zero.' + ENDIF + + IF (CntrPar%IPC_zetaLP <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'IPC_zetaLP must be greater than zero.' + ENDIF + + IF (CntrPar%IPC_zetaNotch <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'IPC_zetaNotch must be greater than zero.' + ENDIF + + IF (CntrPar%Y_ErrThresh <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'Y_ErrThresh must be greater than zero.' + ENDIF + + IF (CntrPar%Y_Rate <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'CntrPar%Y_Rate must be greater than zero.' + ENDIF + + IF (CntrPar%Y_omegaLPFast <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'Y_omegaLPFast must be greater than zero.' + ENDIF + + IF (CntrPar%Y_omegaLPSlow <= 0.0) THEN + aviFAIL = -1 + ErrMsg = 'Y_omegaLPSlow must be greater than zero.' + ENDIF + + ! Abort if the user has not requested a pitch angle actuator (See Appendix A + ! of Bladed User's Guide): + IF (NINT(avrSWAP(10)) /= 0) THEN ! .TRUE. if a pitch angle actuator hasn't been requested + aviFAIL = -1 + ErrMsg = 'Pitch angle actuator not requested.' + ENDIF + + END SUBROUTINE Assert + + SUBROUTINE SetParameters(avrSWAP, aviFAIL, ErrMsg, size_avcMSG, CntrPar, LocalVar, objInst) + + USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances + + INTEGER(4), INTENT(IN) :: size_avcMSG + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + + REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. + INTEGER(C_INT), INTENT(OUT) :: aviFAIL ! A flag used to indicate the success of this DLL call set as follows: 0 if the DLL call was successful, >0 if the DLL call was successful but cMessage should be issued as a warning messsage, <0 if the DLL call was unsuccessful or for any other reason the simulation is to be stopped at this point with cMessage as the error message. + CHARACTER(size_avcMSG-1), INTENT(OUT) :: ErrMsg ! a Fortran version of the C string argument (not considered an array here) [subtract 1 for the C null-character] + + ! Set aviFAIL to 0 in each iteration: + aviFAIL = 0 + + ! Initialize all filter instance counters at 1 + objInst%instLPF = 1 + objInst%instSecLPF = 1 + objInst%instHPF = 1 + objInst%instNotchSlopes = 1 + objInst%instNotch = 1 + + ! Set unused outputs to zero (See Appendix A of Bladed User's Guide): + avrSWAP(36) = 0.0 ! Shaft brake status: 0=off + avrSWAP(41) = 0.0 ! Demanded yaw actuator torque + avrSWAP(46) = 0.0 ! Demanded pitch rate (Collective pitch) + avrSWAP(65) = 0.0 ! Number of variables returned for logging + avrSWAP(72) = 0.0 ! Generator start-up resistance + avrSWAP(79) = 0.0 ! Request for loads: 0=none + avrSWAP(80) = 0.0 ! Variable slip current status + avrSWAP(81) = 0.0 ! Variable slip current demand + + ! Read any External Controller Parameters specified in the User Interface + ! and initialize variables: + IF (LocalVar%iStatus == 0) THEN ! .TRUE. if we're on the first call to the DLL + + ! Inform users that we are using this user-defined routine: + aviFAIL = 1 + ErrMsg = ' '//NEW_LINE('A')// & + 'Running the Delft Research Controller (DRC) '//NEW_LINE('A')// & + 'A wind turbine controller for use in the scientific field '//NEW_LINE('A')// & + 'Written by S.P. Mulders, Jan-Willem van Wingerden '//NEW_LINE('A')// & + 'Delft University of Technology, The Netherlands '//NEW_LINE('A')// & + 'Visit our GitHub-page to contribute to this project: '//NEW_LINE('A')// & + 'https://github.com/TUDelft-DataDrivenControl ' + + CALL ReadControlParameterFileSub(CntrPar) + + ! Initialize testValue (debugging variable) + LocalVar%TestType = 0 + + ! Initialize the SAVEd variables: + ! NOTE: LocalVar%VS_LastGenTrq, though SAVEd, is initialized in the torque controller + ! below for simplicity, not here. + LocalVar%PitCom = LocalVar%BlPitch ! This will ensure that the variable speed controller picks the correct control region and the pitch controller picks the correct gain on the first call + LocalVar%Y_AccErr = 0.0 ! This will ensure that the accumulated yaw error starts at zero + LocalVar%Y_YawEndT = -1.0 ! This will ensure that the initial yaw end time is lower than the actual time to prevent initial yawing + + ! Check validity of input parameters: + CALL Assert(LocalVar, CntrPar, avrSWAP, aviFAIL, ErrMsg, size_avcMSG) + + ENDIF + END SUBROUTINE SetParameters +END MODULE ReadSetParameters \ No newline at end of file diff --git a/Source/makefile b/Source/makefile index 05f008b8..aa3b2c85 100644 --- a/Source/makefile +++ b/Source/makefile @@ -19,7 +19,7 @@ BITS = 32 DLL_DIR = . -SOURCE_FILE = DRC_Types.f90 ReadParameters.f90 FunctionToolbox.f90 Filters.f90 IPC.f90 DISCON.f90 +SOURCE_FILE = Constants.f90 DRC_Types.f90 FunctionToolbox.f90 Filters.f90 ReadSetParameters.f90 Controllers.f90 DISCON.f90 # Name of compiler to use and flags to use.