Skip to content

Commit

Permalink
Add initial ballast controller
Browse files Browse the repository at this point in the history
  • Loading branch information
dzalkind committed Oct 15, 2024
1 parent 99d2819 commit 62db94d
Show file tree
Hide file tree
Showing 7 changed files with 179 additions and 37 deletions.
37 changes: 37 additions & 0 deletions rosco/controller/rosco_registry/rosco_types.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -669,6 +669,31 @@ ControlParameters:
description: Cable control group indices
allocatable: True

StC_F_FiltFreq:
<<: *real
description: Frequency of 1st order LPF filter on pitch/roll motion
StC_F_PID:
<<: *real
description: PID gains of controller from pitch/roll to force
allocatable: True
StC_F_Lims:
<<: *real
description: Min and max force at each leg
allocatable: True
StC_F_Rates:
<<: *real
description: Min and max force rates (at each leg?)
allocatable: True
StC_T_Roll:
<<: *real
description: Transformation from F_Roll (output of PID control) to StC_Force (length should match StC_Group_N), will be normalized to max of 1, must sum to 0
allocatable: True
StC_T_Pitch:
<<: *real
description: Transformation from F_Pitch (output of PID control) to StC_Force (length should match StC_Group_N), will be normalized to max of 1, must sum to 0
allocatable: True


# Calculated
PC_RtTq99:
<<: *real
Expand Down Expand Up @@ -1275,6 +1300,18 @@ LocalVariables:
PtfmRAZ:
<<: *real
description: Platform motion -- Acceleration RAZ (rad/s^2)')
PtfmRDX_F:
<<: *real
description: Filtered Platform motion -- Displacement RDX (rad)')
PtfmRDY_F:
<<: *real
description: Filtered Platform motion -- Displacement RDY (rad)')
Force_Pitch:
<<: *real
description: Force to pitch the rotor back to even keel
Force_Roll:
<<: *real
description: Force to roll the rotor back to even keel
CC_DesiredL:
<<: *real
size: 12
Expand Down
36 changes: 29 additions & 7 deletions rosco/controller/src/Controllers.f90
Original file line number Diff line number Diff line change
Expand Up @@ -825,6 +825,11 @@ SUBROUTINE StructuralControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar)
TYPE(ObjectInstances), INTENT(INOUT) :: objInst
TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar

REAL(DbKi), DIMENSION(6,2) :: T ! Transformation from pitch/roll to 6 legs
REAL(DbKi), DIMENSION(2,1) :: Inp ! Transformation from pitch/roll to 6 legs
REAL(DbKi), DIMENSION(6,1) :: F_Out ! Transformation from pitch/roll to 6 legs



! Internal Variables
Integer(IntKi) :: I_GROUP
Expand All @@ -833,15 +838,32 @@ SUBROUTINE StructuralControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar)


IF (CntrPar%StC_Mode == 1) THEN
! User defined control, step example
! Ballast control

IF (LocalVar%Time > 500) THEN
! Step change in input of -4500 N
LocalVar%StC_Input(1) = -1.234e+06
LocalVar%StC_Input(2) = 2.053e+06
LocalVar%StC_Input(3) = -7.795e+05
! Filter pitch and roll offsets
LocalVar%PtfmRDX_F = LPFilter(LocalVar%PtfmRDX, LocalVar%DT, CntrPar%StC_F_FiltFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ! roll
LocalVar%PtfmRDY_F = LPFilter(LocalVar%PtfmRDY, LocalVar%DT, CntrPar%StC_F_FiltFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ! pitch

! Pitch and roll force commands

! PIDController(error, kp, ki, kd, tf, minValue, maxValue, DT, I0, piP, reset, objInst, LocalVar)

LocalVar%Force_Roll = PIDController(LocalVar%PtfmRDX_F, CntrPar%StC_F_PID(1), CntrPar%StC_F_PID(2), CntrPar%StC_F_PID(3), 100.0_DbKi, CntrPar%StC_F_Lims(1), CntrPar%StC_F_Lims(2), LocalVar%DT, 0.0, LocalVar%piP, LocalVar%restart, objInst, LocalVar)
LocalVar%Force_Pitch = PIDController(LocalVar%PtfmRDY_F, CntrPar%StC_F_PID(1), CntrPar%StC_F_PID(2), CntrPar%StC_F_PID(3), 100.0_DbKi, CntrPar%StC_F_Lims(1), CntrPar%StC_F_Lims(2), LocalVar%DT, 0.0, LocalVar%piP, LocalVar%restart, objInst, LocalVar)

T = RESHAPE((/CntrPar%StC_T_Roll, CntrPar%StC_T_Pitch/),(/6,2/))
! T = RESHAPE((/0.0, 0.0, 0.0, 0.0, -0.0, -0.0, -1.0, -0.5, 0.5, 1.0, 0.5, -0.5/),(/6,2/)) ! 0 roll correction
Inp = RESHAPE( (/LocalVar%Force_Roll, LocalVar%Force_Pitch/) , (/2,1/))
F_Out = matmul(T,Inp)


! WRITE(400,*) LocalVar%Time, LocalVar%PtfmRDX_F, LocalVar%PtfmRDY_F, F_Out

! Assign to StC_Input
DO I_GROUP = 1,CntrPar%StC_Group_N
LocalVar%StC_Input(I_GROUP) = F_Out(I_GROUP,1)
END DO

END IF


ELSEIF (CntrPar%StC_Mode == 2) THEN
Expand Down
67 changes: 40 additions & 27 deletions rosco/controller/src/ROSCO_IO.f90
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -169,6 +169,10 @@ SUBROUTINE WriteRestartFile(LocalVar, CntrPar, ErrVar, objInst, RootName, size_a
WRITE( Un, IOSTAT=ErrStat) LocalVar%PtfmRAX
WRITE( Un, IOSTAT=ErrStat) LocalVar%PtfmRAY
WRITE( Un, IOSTAT=ErrStat) LocalVar%PtfmRAZ
WRITE( Un, IOSTAT=ErrStat) LocalVar%PtfmRDX_F
WRITE( Un, IOSTAT=ErrStat) LocalVar%PtfmRDY_F
WRITE( Un, IOSTAT=ErrStat) LocalVar%Force_Pitch
WRITE( Un, IOSTAT=ErrStat) LocalVar%Force_Roll
WRITE( Un, IOSTAT=ErrStat) LocalVar%CC_DesiredL(1)
WRITE( Un, IOSTAT=ErrStat) LocalVar%CC_DesiredL(2)
WRITE( Un, IOSTAT=ErrStat) LocalVar%CC_DesiredL(3)
Expand Down Expand Up @@ -471,6 +475,10 @@ SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootNa
READ( Un, IOSTAT=ErrStat) LocalVar%PtfmRAX
READ( Un, IOSTAT=ErrStat) LocalVar%PtfmRAY
READ( Un, IOSTAT=ErrStat) LocalVar%PtfmRAZ
READ( Un, IOSTAT=ErrStat) LocalVar%PtfmRDX_F
READ( Un, IOSTAT=ErrStat) LocalVar%PtfmRDY_F
READ( Un, IOSTAT=ErrStat) LocalVar%Force_Pitch
READ( Un, IOSTAT=ErrStat) LocalVar%Force_Roll
READ( Un, IOSTAT=ErrStat) LocalVar%CC_DesiredL(1)
READ( Un, IOSTAT=ErrStat) LocalVar%CC_DesiredL(2)
READ( Un, IOSTAT=ErrStat) LocalVar%CC_DesiredL(3)
Expand Down Expand Up @@ -692,7 +700,7 @@ 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 = 128
Allocate(LocalVarOutData(nLocalVars))
Allocate(LocalVarOutStrings(nLocalVars))
LocalVarOutData(1) = LocalVar%iStatus
Expand Down Expand Up @@ -807,18 +815,22 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, ErrVar, avrSWAP, RootName, size_av
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)
LocalVarOutData(113) = LocalVar%PtfmRDX_F
LocalVarOutData(114) = LocalVar%PtfmRDY_F
LocalVarOutData(115) = LocalVar%Force_Pitch
LocalVarOutData(116) = LocalVar%Force_Roll
LocalVarOutData(117) = LocalVar%CC_DesiredL(1)
LocalVarOutData(118) = LocalVar%CC_ActuatedL(1)
LocalVarOutData(119) = LocalVar%CC_ActuatedDL(1)
LocalVarOutData(120) = LocalVar%StC_Input(1)
LocalVarOutData(121) = LocalVar%Flp_Angle(1)
LocalVarOutData(122) = LocalVar%RootMyb_Last(1)
LocalVarOutData(123) = LocalVar%ACC_INFILE_SIZE
LocalVarOutData(124) = LocalVar%AWC_complexangle(1)
LocalVarOutData(125) = LocalVar%ZMQ_ID
LocalVarOutData(126) = LocalVar%ZMQ_YawOffset
LocalVarOutData(127) = LocalVar%ZMQ_TorqueOffset
LocalVarOutData(128) = 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', &
Expand All @@ -841,9 +853,10 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, ErrVar, avrSWAP, RootName, size_av
'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']
'PtfmRAY', 'PtfmRAZ', 'PtfmRDX_F', 'PtfmRDY_F', 'Force_Pitch', &
'Force_Roll', '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']
! 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
Expand All @@ -858,8 +871,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, '(129(a20,TR5:))') 'Time', LocalVarOutStrings
WRITE(UnDb2, '(129(a20,TR5:))')
END IF

IF (CntrPar%LoggingLevel > 2) THEN
Expand Down Expand Up @@ -904,25 +917,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,128(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
Expand All @@ -945,4 +958,4 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, ErrVar, avrSWAP, RootName, size_av
ENDIF
END SUBROUTINE Debug

END MODULE ROSCO_IO
END MODULE ROSCO_IO
12 changes: 11 additions & 1 deletion rosco/controller/src/ROSCO_Types.f90
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -170,6 +170,12 @@ MODULE ROSCO_Types
INTEGER(IntKi) :: StC_Mode ! Flag for StC Control
INTEGER(IntKi) :: StC_Group_N ! Number of cable control groups
INTEGER(IntKi), DIMENSION(:), ALLOCATABLE :: StC_GroupIndex ! Cable control group indices
REAL(DbKi) :: StC_F_FiltFreq ! Frequency of 1st order LPF filter on pitch/roll motion
REAL(DbKi), DIMENSION(:), ALLOCATABLE :: StC_F_PID ! PID gains of controller from pitch/roll to force
REAL(DbKi), DIMENSION(:), ALLOCATABLE :: StC_F_Lims ! Min and max force at each leg
REAL(DbKi), DIMENSION(:), ALLOCATABLE :: StC_F_Rates ! Min and max force rates (at each leg?)
REAL(DbKi), DIMENSION(:), ALLOCATABLE :: StC_T_Roll ! Transformation from F_Roll (output of PID control) to StC_Force (length should match StC_Group_N), will be normalized to max of 1, must sum to 0
REAL(DbKi), DIMENSION(:), ALLOCATABLE :: StC_T_Pitch ! Transformation from F_Pitch (output of PID control) to StC_Force (length should match StC_Group_N), will be normalized to max of 1, must sum to 0
REAL(DbKi) :: PC_RtTq99 ! 99% of the rated torque value, using for switching between pitch and torque control, [Nm].
REAL(DbKi) :: VS_MaxOMTq ! Maximum torque at the end of the below-rated region 2, [Nm]
REAL(DbKi) :: VS_MinOMTq ! Minimum torque at the beginning of the below-rated region 2, [Nm]
Expand Down Expand Up @@ -361,6 +367,10 @@ MODULE ROSCO_Types
REAL(DbKi) :: PtfmRAX ! Platform motion -- Acceleration RAX (rad/s^2)')
REAL(DbKi) :: PtfmRAY ! Platform motion -- Acceleration RAY (rad/s^2)')
REAL(DbKi) :: PtfmRAZ ! Platform motion -- Acceleration RAZ (rad/s^2)')
REAL(DbKi) :: PtfmRDX_F ! Filtered Platform motion -- Displacement RDX (rad)')
REAL(DbKi) :: PtfmRDY_F ! Filtered Platform motion -- Displacement RDY (rad)')
REAL(DbKi) :: Force_Pitch ! Force to pitch the rotor back to even keel
REAL(DbKi) :: Force_Roll ! Force to roll the rotor back to even keel
REAL(DbKi) :: CC_DesiredL(12) ! None
REAL(DbKi) :: CC_ActuatedL(12) ! None
REAL(DbKi) :: CC_ActuatedDL(12) ! None
Expand Down
22 changes: 20 additions & 2 deletions rosco/controller/src/ReadSetParameters.f90
Original file line number Diff line number Diff line change
Expand Up @@ -554,8 +554,15 @@ SUBROUTINE ReadControlParameterFileSub(CntrPar, LocalVar, accINFILE, accINFILE_s
IF (ErrVar%aviFAIL < 0) RETURN

!------------- StC Control -----
CALL ParseInput(FileLines, 'StC_Group_N', CntrPar%StC_Group_N, accINFILE(1), ErrVar, CntrPar%StC_Mode == 0, UnEc)
CALL ParseAry( FileLines, 'StC_GroupIndex', CntrPar%StC_GroupIndex, CntrPar%StC_Group_N, accINFILE(1), ErrVar, CntrPar%StC_Mode == 0, UnEc)
CALL ParseInput(FileLines, 'StC_Group_N', CntrPar%StC_Group_N, accINFILE(1), ErrVar, CntrPar%StC_Mode == 0, UnEc)
CALL ParseAry( FileLines, 'StC_GroupIndex', CntrPar%StC_GroupIndex, CntrPar%StC_Group_N, accINFILE(1), ErrVar, CntrPar%StC_Mode == 0, UnEc)
CALL ParseInput(FileLines, 'StC_F_FiltFreq', CntrPar%StC_F_FiltFreq, accINFILE(1), ErrVar, CntrPar%StC_Mode .NE. 1, UnEc)
CALL ParseAry(FileLines, 'StC_F_PID', CntrPar%StC_F_PID, 3, accINFILE(1), ErrVar, CntrPar%StC_Mode .NE. 1, UnEc)
CALL ParseAry(FileLines, 'StC_F_Lims', CntrPar%StC_F_Lims, 2, accINFILE(1), ErrVar, CntrPar%StC_Mode .NE. 1, UnEc)
CALL ParseAry(FileLines, 'StC_F_Rates', CntrPar%StC_F_Rates, 2, accINFILE(1), ErrVar, CntrPar%StC_Mode .NE. 1, UnEc)
CALL ParseAry(FileLines, 'StC_T_Roll', CntrPar%StC_T_Roll, CntrPar%StC_Group_N, accINFILE(1), ErrVar, CntrPar%StC_Mode .NE. 1, UnEc)
CALL ParseAry(FileLines, 'StC_T_Pitch', CntrPar%StC_T_Pitch, CntrPar%StC_Group_N, accINFILE(1), ErrVar, CntrPar%StC_Mode .NE. 1, UnEc)

IF (ErrVar%aviFAIL < 0) RETURN

! Open loop cable, structural control, needs number of groups
Expand Down Expand Up @@ -1461,6 +1468,17 @@ SUBROUTINE CheckInputs(LocalVar, CntrPar, avrSWAP, ErrVar, size_avcMSG)
ErrVar%ErrMsg = 'StC_GroupIndices must be greater than 2801.' !< Starting index for the cable control
END IF
END DO

IF (ABS(SUM(CntrPar%StC_T_Roll)) > 1e-4) THEN
ErrVar%aviFAIL = -1
ErrVar%ErrMsg = 'StC_T_Roll must sum to 0.'
END IF

IF (ABS(SUM(CntrPar%StC_T_Pitch)) > 1e-4) THEN
ErrVar%aviFAIL = -1
ErrVar%ErrMsg = 'StC_T_Pitch must sum to 0.'
END IF

END IF

! Check that open loop control active if using open loop cable/struct control
Expand Down
34 changes: 34 additions & 0 deletions rosco/toolbox/inputs/toolbox_schema.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -1150,6 +1150,40 @@ properties:
type: number
description: Integer identifier of turbine
default: 0
StC_F_FiltFreq:
type: number
description: Frequency of 1st order LPF filter on pitch/roll motion
default: 0.01
StC_F_PID:
type: array
items:
type: number
description: PID gains of controller from pitch/roll to force
default: [0,0,0]
StC_F_Lims:
type: array
items:
type: number
description: Min and max force at each leg
default: [-1,1]
StC_F_Rates:
type: array
items:
type: number
description: Min and max force rates (at each leg?)
default: [-1,1]
StC_T_Roll:
type: array
items:
type: number
description: Transformation from F_Roll (output of PID control) to StC_Force (length should match StC_Group_N), will be normalized to max of 1, must sum to 0
default: [0,0,0,0,0,0]
StC_T_Pitch:
type: array
items:
type: number
description: Transformation from F_Pitch (output of PID control) to StC_Force (length should match StC_Group_N), will be normalized to max of 1, must sum to 0
default: [0,0,0,0,0,0]

linmodel_tuning:
type: object
Expand Down
Loading

0 comments on commit 62db94d

Please sign in to comment.