Skip to content

Commit

Permalink
Put rate limit inside PID controller
Browse files Browse the repository at this point in the history
  • Loading branch information
@dzalkind
dzalkind committed Nov 21, 2024
1 parent 26ccd3d commit 12968a8
Show file tree
Hide file tree
Showing 2 changed files with 71 additions and 4 deletions.
12 changes: 8 additions & 4 deletions rosco/controller/src/Controllers.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -846,13 +846,17 @@ SUBROUTINE StructuralControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar)

! Pitch and roll force commands

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)
! 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)

LocalVar%Force_Roll = PIDControllerR(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), CntrPar%StC_F_Rates(1), CntrPar%StC_F_Rates(2), LocalVar%DT, 0.0, LocalVar%piP, LocalVar%restart, objInst, LocalVar)
LocalVar%Force_Pitch = PIDControllerR(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), CntrPar%StC_F_Rates(1), CntrPar%StC_F_Rates(2), LocalVar%DT, 0.0, LocalVar%piP, LocalVar%restart, objInst, LocalVar)



! Rate limit
LocalVar%Force_Roll = ratelimit(LocalVar%Force_Roll,CntrPar%StC_F_Rates(1),CntrPar%StC_F_Rates(2), LocalVar%DT, LocalVar%restart,LocalVar%rlP, objInst%instRL)
LocalVar%Force_Pitch = ratelimit(LocalVar%Force_Pitch,CntrPar%StC_F_Rates(1),CntrPar%StC_F_Rates(2), LocalVar%DT, LocalVar%restart,LocalVar%rlP, objInst%instRL)
! LocalVar%Force_Roll = ratelimit(LocalVar%Force_Roll,CntrPar%StC_F_Rates(1),CntrPar%StC_F_Rates(2), LocalVar%DT, LocalVar%restart,LocalVar%rlP, objInst%instRL)
! LocalVar%Force_Pitch = ratelimit(LocalVar%Force_Pitch,CntrPar%StC_F_Rates(1),CntrPar%StC_F_Rates(2), LocalVar%DT, LocalVar%restart,LocalVar%rlP, objInst%instRL)

T = RESHAPE((/CntrPar%StC_T_Roll, CntrPar%StC_T_Pitch/),(/6,2/))
Inp = RESHAPE( (/LocalVar%Force_Roll, LocalVar%Force_Pitch/) , (/2,1/))
Expand Down
63 changes: 63 additions & 0 deletions rosco/controller/src/Functions.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,6 +191,69 @@ REAL(DbKi) FUNCTION PIDController(error, kp, ki, kd, tf, minValue, maxValue, DT,

END FUNCTION PIDController

!-------------------------------------------------------------------------------------------------------------------------------
REAL(DbKi) FUNCTION PIDControllerR(error, kp, ki, kd, tf, minValue, maxValue, minRate, maxRate, DT, I0, piP, reset, objInst, LocalVar)
USE ROSCO_Types, ONLY : piParams, LocalVariables, ObjectInstances

! PI controller, with output saturation

IMPLICIT NONE
! Allocate Inputs
REAL(DbKi), INTENT(IN) :: error
REAL(DbKi), INTENT(IN) :: kp
REAL(DbKi), INTENT(IN) :: ki
REAL(DbKi), INTENT(IN) :: kd
REAL(DbKi), INTENT(IN) :: tf
REAL(DbKi), INTENT(IN) :: minValue, minRate
REAL(DbKi), INTENT(IN) :: maxValue, maxRate
REAL(DbKi), INTENT(IN) :: DT
TYPE(ObjectInstances), INTENT(INOUT) :: objInst ! all object instances (PI, filters used here)
TYPE(LocalVariables), INTENT(INOUT) :: LocalVar

REAL(DbKi), INTENT(IN) :: I0
TYPE(piParams), INTENT(INOUT) :: piP
LOGICAL, INTENT(IN) :: reset

! Allocate local variables
INTEGER(IntKi) :: i ! Counter for making arrays
REAL(DbKi) :: PTerm, DTerm ! Proportional, deriv. terms
REAL(DbKi) :: EFilt ! Filtered error for derivative
REAL(DbKi) :: Output ! Filtered error for derivative

! Always filter error
EFilt = LPFilter(error, DT, tf, LocalVar%FP, LocalVar%iStatus, reset, objInst%instLPF)

! Initialize persistent variables/arrays, and set inital condition for integrator term
IF (reset) THEN
piP%ITerm(objInst%instPI) = I0
piP%ITermLast(objInst%instPI) = I0
piP%ELast(objInst%instPI) = 0.0_DbKi
PIDControllerR = I0
ELSE
! Proportional
PTerm = kp*error

! Integrate and saturate
piP%ITerm(objInst%instPI) = piP%ITerm(objInst%instPI) + DT*ki*error
piP%ITerm(objInst%instPI) = ratelimit(piP%ITerm(objInst%instPI),minRate, maxRate, DT, reset,LocalVar%rlP, objInst%instRL)
piP%ITerm(objInst%instPI) = saturate(piP%ITerm(objInst%instPI), minValue, maxValue)

! Derivative (filtered)
DTerm = kd * (EFilt - piP%ELast(objInst%instPI)) / DT

! Saturate all
output = ratelimit(PTerm + piP%ITerm(objInst%instPI) + DTerm,minRate, maxRate, DT, reset,LocalVar%rlP, objInst%instRL)

PIDControllerR = saturate(output, minValue, maxValue)

! Save lasts
piP%ITermLast(objInst%instPI) = piP%ITerm(objInst%instPI)
piP%ELast(objInst%instPI) = EFilt
END IF
objInst%instPI = objInst%instPI + 1

END FUNCTION PIDControllerR

!-------------------------------------------------------------------------------------------------------------------------------
REAL(DbKi) FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, DT, I0, piP, reset, inst)
! PI controller, with output saturation.
Expand Down

0 comments on commit 12968a8

Please sign in to comment.