Added notch filter in Filters.f90, added DFController in FunctionTool…

…box.f90 (derivative term with first order filter)
Ben-Mertz · Sep 8, 2017 · 590e008 · 590e008
1 parent ff6e0d8
commit 590e008
Show file tree

Hide file tree

Showing 4 changed files with 121 additions and 62 deletions.
diff --git a/Source/DISCON.f90 b/Source/DISCON.f90
@@ -40,7 +40,7 @@ SUBROUTINE DISCON (avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAM
 
 REAL(4)						:: Azimuth											! Rotor azimuth angle [rad].
 REAL(4)						:: BlPitch(3)										! Current values of the blade pitch angles [rad].
-REAL(4), PARAMETER			:: CornerFreq = 0.7853981							! Corner frequency (-3dB point) in the first-order low-pass filter, [rad/s]
+REAL(4), PARAMETER			:: CornerFreq = 3.1415926							! Corner frequency (-3dB point) in the first-order low-pass filter, [rad/s]
 REAL(4)						:: DT												! Time step [s].
 REAL(4)						:: ElapTime											! Elapsed time since the last call to the controller [s].
 REAL(4)						:: GenSpeed											! Current  HSS (generator) speed [rad/s].
@@ -63,10 +63,14 @@ SUBROUTINE DISCON (avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAM
 REAL(4)						:: PC_GK											! Current value of the gain correction factor, used in the gain scheduling law of the pitch controller, [-].
 INTEGER(4), SAVE							:: PC_GS_n							! Amount of gain-scheduling table entries
 REAL(4), DIMENSION(:), ALLOCATABLE, SAVE	:: PC_GS_angles						! Gain-schedule table: pitch angles
-REAL(4), DIMENSION(:), ALLOCATABLE, SAVE	:: PC_GS_kp							! Gain-schedule table: pitch controller kp gainspitch angles
-REAL(4), DIMENSION(:), ALLOCATABLE, SAVE	:: PC_GS_ki							! Gain-schedule table: pitch controller ki gainspitch angles
-REAL(4)						:: PC_KI											! Integral gain for pitch controller at rated pitch (zero), [-].
+REAL(4), DIMENSION(:), ALLOCATABLE, SAVE	:: PC_GS_kp							! Gain-schedule table: pitch controller kp gains
+REAL(4), DIMENSION(:), ALLOCATABLE, SAVE	:: PC_GS_ki							! Gain-schedule table: pitch controller ki gains
+REAL(4), DIMENSION(:), ALLOCATABLE, SAVE	:: PC_GS_kd							! Gain-schedule table: pitch controller kd gains
+REAL(4), DIMENSION(:), ALLOCATABLE, SAVE	:: PC_GS_tf							! Gain-schedule table: pitch controller tf gains (derivative filter)
 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											! 
+REAL(4)						:: PC_TF											! 
 REAL(4)						:: PC_MaxPit										! Maximum physical pitch limit, [rad].
 REAL(4)						:: PC_MaxPitVar										! Maximum pitch setting in pitch controller (variable) [rad].
 REAL(4)						:: PC_MaxRat										! Maximum pitch rate (in absolute value) in pitch controller, [rad/s].
@@ -234,6 +238,12 @@ SUBROUTINE DISCON (avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAM
 	ALLOCATE(PC_GS_ki(PC_GS_n))
 	READ(UnPitchGains,*) PC_GS_ki
 
+	ALLOCATE(PC_GS_kd(PC_GS_n))
+	READ(UnPitchGains,*) PC_GS_kd
+
+	ALLOCATE(PC_GS_tf(PC_GS_n))
+	READ(UnPitchGains,*) PC_GS_tf
+
 	READ(UnPitchGains, *) VS_n
 
 	ALLOCATE(VS_KP(VS_n))
@@ -485,16 +495,18 @@ SUBROUTINE DISCON (avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAM
 
 	PC_KP = interp1d(PC_GS_angles, PC_GS_kp, PitComT)
 	PC_KI = interp1d(PC_GS_angles, PC_GS_ki, PitComT)
+	PC_KD = interp1d(PC_GS_angles, PC_GS_kd, PitComT)
+	PC_TF = interp1d(PC_GS_angles, 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:
 
-	PC_SpdErr = PC_RefSpd - GenSpeedF									! Current speed error
+	PC_SpdErr = PC_RefSpd - GenSpeedF									! Current speed error [UNFILTERED SIGNAL!!]
 
 		! Compute the pitch commands associated with the proportional and integral
 		!   gains:
 
-	PitComT = PIController(PC_SpdErr, PC_KP, PC_KI, PC_SetPnt, PC_MaxPitVar, ElapTime, PC_SetPnt, 2)
+	PitComT = PIController(PC_SpdErr, PC_KP, PC_KI, PC_SetPnt, PC_MaxPitVar, ElapTime, PC_SetPnt, 2) + DFController(PC_SpdErr, PC_KD, PC_TF, DT, 1)
 
 		! Individual pitch control
 

diff --git a/Source/Filters.f90 b/Source/Filters.f90
@@ -132,50 +132,103 @@ REAL FUNCTION HPFilter( InputSignal, DT, CornerFreq, iStatus, inst)
 
     END FUNCTION HPFilter
     !-------------------------------------------------------------------------------------------------------------------------------
-    ! Discrete time inverted Notch Filter
-    REAL FUNCTION NotchFilter(InputSignal, DT, CornerFreq, Damp, iStatus, inst)
+    ! Discrete time inverted Notch Filter with descending slopes, G = CornerFreq*s/(Damp*s^2+CornerFreq*s+Damp*CornerFreq^2)
+    REAL FUNCTION NotchFilterSlopes(InputSignal, DT, CornerFreq, Damp, iStatus, inst)
     !...............................................................................................................................
 
         IMPLICIT NONE
 
 			! Inputs
 
-        REAL(4), INTENT(IN)     :: InputSignal
+		REAL(4), INTENT(IN)     :: InputSignal
 		REAL(4), INTENT(IN)     :: DT						! time step [s]
 		REAL(4), INTENT(IN)     :: CornerFreq				! corner frequency [rad/s]
 		REAL(4), INTENT(IN)     :: Damp						! Dampening constant
-        INTEGER, INTENT(IN)		:: 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, INTENT(IN)		:: inst						! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other.
+		INTEGER, INTENT(IN)		:: 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, INTENT(IN)		:: inst						! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other.
 
 			! Local
 
-        REAL(4), DIMENSION(99), SAVE :: InputSignalLast1	! Input signal the last time this filter was called. Supports 99 separate instances.
+		REAL(4), DIMENSION(99), SAVE :: InputSignalLast1	! Input signal the last time this filter was called. Supports 99 separate instances.
 		REAL(4), DIMENSION(99), SAVE :: InputSignalLast2	! Input signal the next to last time this filter was called. Supports 99 separate instances.
-        REAL(4), DIMENSION(99), SAVE :: OutputSignalLast1	! Output signal the last time this filter was called. Supports 99 separate instances.
-        REAL(4), DIMENSION(99), SAVE :: OutputSignalLast2	! Output signal the next to last time this filter was called. Supports 99 separate instances.
+		REAL(4), DIMENSION(99), SAVE :: OutputSignalLast1	! Output signal the last time this filter was called. Supports 99 separate instances.
+		REAL(4), DIMENSION(99), SAVE :: OutputSignalLast2	! Output signal the next to last time this filter was called. Supports 99 separate instances.
 
 			! Initialization
 
-        IF ( iStatus == 0 )  THEN
-            OutputSignalLast1(inst)  = InputSignal
-            OutputSignalLast2(inst)  = InputSignal
-            InputSignalLast1(inst)   = InputSignal
-            InputSignalLast2(inst)   = InputSignal
-        ENDIF
+		IF ( iStatus == 0 ) THEN
+			OutputSignalLast1(inst)  = InputSignal
+			OutputSignalLast2(inst)  = InputSignal
+			InputSignalLast1(inst)   = InputSignal
+			InputSignalLast2(inst)   = InputSignal
+		ENDIF
 
 			! Body
 
-        NotchFilter = 1.0/(4.0+2.0*DT*Damp*CornerFreq+DT**2.0*CornerFreq**2.0) * ( (8.0-2.0*DT**2.0*CornerFreq**2.0)*OutputSignalLast1(inst) &
+		NotchFilterSlopes = 1.0/(4.0+2.0*DT*Damp*CornerFreq+DT**2.0*CornerFreq**2.0) * ( (8.0-2.0*DT**2.0*CornerFreq**2.0)*OutputSignalLast1(inst) &
 						+ (-4.0+2.0*DT*Damp*CornerFreq-DT**2.0*CornerFreq**2.0)*OutputSignalLast2(inst) + &
 							(2.0*DT*Damp*CornerFreq)*InputSignal + (-2.0*DT*Damp*CornerFreq)*InputSignalLast2(inst) )
 
 			! Save signals for next time step
 
-        InputSignalLast2(inst)   = InputSignalLast1(inst)
-        InputSignalLast1(inst)   = InputSignal			!Save input signal for next time step
-        OutputSignalLast2(inst)  = OutputSignalLast1(inst)		!Save input signal for next time step
-        OutputSignalLast1(inst)  = NotchFilter
+		InputSignalLast2(inst)   = InputSignalLast1(inst)
+		InputSignalLast1(inst)   = InputSignal			!Save input signal for next time step
+		OutputSignalLast2(inst)  = OutputSignalLast1(inst)		!Save input signal for next time step
+		OutputSignalLast1(inst)  = NotchFilter
 
-    END FUNCTION NotchFilter
-    !-------------------------------------------------------------------------------------------------------------------------------
-    END MODULE Filters
+	END FUNCTION NotchFilterSlopes
+	!-------------------------------------------------------------------------------------------------------------------------------
+	! Discrete time inverted Notch Filter with descending slopes, G = (s^2 + 2*omega*betaNum*s + omega^2)/(s^2 + 2*omega*betaDen*s + omega^2)
+	REAL FUNCTION NotchFilter(InputSignal, DT, omega, betaNum, betaDen, iStatus, inst)
+	!...............................................................................................................................
+
+		IMPLICIT NONE
+
+			! Inputs
+
+		REAL(4), INTENT(IN)     :: InputSignal
+		REAL(4), INTENT(IN)     :: DT						! time step [s]
+		REAL(4), INTENT(IN)     :: omega					! corner frequency [rad/s]
+		REAL(4), INTENT(IN)     :: betaNum					! Dampening constant in numerator of filter transfer function
+		REAL(4), INTENT(IN)     :: betaDen					! Dampening constant in denominator of filter transfer function
+		INTEGER, INTENT(IN)		:: 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, INTENT(IN)		:: inst						! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other.
+
+			! Local
+		REAL(4)							:: K, P1, P2, P3, P4, P5	! Constant gain
+		REAL(4), DIMENSION(99), SAVE	:: InputSignalLast1			! Input signal the last time this filter was called. Supports 99 separate instances.
+		REAL(4), DIMENSION(99), SAVE	:: InputSignalLast2			! Input signal the next to last time this filter was called. Supports 99 separate instances.
+		REAL(4), DIMENSION(99), SAVE	:: OutputSignalLast1		! Output signal the last time this filter was called. Supports 99 separate instances.
+		REAL(4), DIMENSION(99), SAVE	:: OutputSignalLast2		! Output signal the next to last time this filter was called. Supports 99 separate instances.
+
+			! Initialization
+
+		IF ( iStatus == 0 ) THEN
+			OutputSignalLast1(inst)  = InputSignal
+			OutputSignalLast2(inst)  = InputSignal
+			InputSignalLast1(inst)   = InputSignal
+			InputSignalLast2(inst)   = InputSignal
+		ENDIF
+
+		K = 2/DT
+		P1 = (K**2 + 2*omega*BetaNum*K + omega**2)/(K**2 + 2*omega*BetaDen*K + omega**2)
+		P2 = (2*omega**2 - 2*K**2)  / (K**2 + 2*omega*BetaDen*K + omega**2);
+		P3 = (K**2 - 2*omega*BetaNum*K + omega**2) / (K**2 + 2*omega*BetaDen*K + omega**2)
+		P4 = (2*omega**2 - 2*K**2)  / (K**2 + 2*omega*BetaDen*K + omega**2)
+		P5 = (K**2 - 2*omega*BetaDen*K + omega**2)/ (K**2 + 2*omega*BetaDen*K + omega**2)
+
+			! Body
+
+
+		NotchFilter = P1*InputSignal + P2*InputSignalLast1(inst) + P3*InputSignalLast2(inst) - P4*OutputSignalLast1(inst) - P5*OutputSignalLast2(inst)
+
+			! Save signals for next time step
+
+		InputSignalLast2(inst)   = InputSignalLast1(inst)
+		InputSignalLast1(inst)   = InputSignal			!Save input signal for next time step
+		OutputSignalLast2(inst)  = OutputSignalLast1(inst)		!Save input signal for next time step
+		OutputSignalLast1(inst)  = NotchFilter
+
+	END FUNCTION NotchFilter
+	!-------------------------------------------------------------------------------------------------------------------------------
+	END MODULE Filters
diff --git a/Source/FunctionToolbox.f90 b/Source/FunctionToolbox.f90
@@ -61,12 +61,6 @@ REAL FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, inst)
 		REAL(4), DIMENSION(99), SAVE	:: ITermLast							! Integral term, the last time this controller was called. Supports 99 separate instances.
 		INTEGER(4), DIMENSION(99), SAVE	:: FirstCall = (/ (1, i=1,99) /)		! First call of this function?
 
-			! Debugging
-		! INTEGER(4), PARAMETER			:: UnDb = 90									! I/O unit for the debugging information
-		! CHARACTER(LEN=6), PARAMETER	:: RootName = "PI_DBG"							! 
-		! 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
-
 			! Initialize persistent variables/arrays, and set inital condition for integrator term
 		IF (FirstCall(inst) == 1) THEN
 			ITerm(1:99) = (/ (real(9999.9), i = 1,99) /)
@@ -75,10 +69,6 @@ REAL FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, inst)
 			ITerm(inst) = I0
 			ITermLast(inst) = I0
 
-			! OPEN ( UnDb, FILE=TRIM( RootName )//'.dbg', STATUS='REPLACE' )
-			! WRITE (UnDb,'(/////)')
-			! WRITE (UnDb,'(A)')  'FirstCall(inst)'  //Tab//'ITerm(inst)' //Tab//'error ' //Tab//'kp  ' //Tab//'ki ' //Tab//'DT'
-
 			FirstCall(inst) = 0
 		END IF
 
@@ -92,8 +82,6 @@ REAL FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, inst)
 
 		! WRITE (UnDb,FmtDat)  real(FirstCall(inst)),	ITerm(inst),	error,	kp,	ki,	DT
 	END FUNCTION PIController
-	!-------------------------------------------------------------------------------------------------------------------------------
-
 	!-------------------------------------------------------------------------------------------------------------------------------
 	! interp1 1-D interpolation (table lookup), xData and yData should be monotonically increasing
 	REAL FUNCTION interp1d(xData, yData, xq)
@@ -122,29 +110,35 @@ REAL FUNCTION interp1d(xData, yData, xq)
 
 	END FUNCTION interp1d
 	!-------------------------------------------------------------------------------------------------------------------------------
-	! Read gain gain scheduled pitch gains from file PitchGains.IN
-	! SUBROUTINE readPitchGains(angles, kp, ki)
-
-		! IMPLICIT NONE
-
-		! REAL, DIMENSION(:), ALLOCATABLE, INTENT(OUT)	:: angles, kp, ki
-		! INTEGER											:: n
+	! PI controller, with output saturation
+	REAL FUNCTION DFController(error, Kd, Tf, DT, inst)
+	!
+		IMPLICIT NONE
 
-		! OPEN(unit=99, file='PitchGains.IN', status='old', action='read')
-		! READ(99, *) n
-
-		! ALLOCATE(angles(n))
-		! READ(99,*) angles
-		! WRITE(*,*) angles
+			! Inputs
+		REAL(4), INTENT(IN)		:: error
+		REAL(4), INTENT(IN)		:: kd
+		REAL(4), INTENT(IN)		:: tf
+		REAL(4), INTENT(IN)		:: DT
+		INTEGER(4), INTENT(IN)	:: inst
 
-		! ALLOCATE(kp(n))
-		! READ(99,*) kp
-		! WRITE(*,*) kp
+			! Local
+		REAL(4)							:: B									! 
+		INTEGER(4)						:: i									! Counter for making arrays
+		REAL(4), DIMENSION(99), SAVE	:: errorLast = (/ (0, i=1,99) /)		! 
+		REAL(4), DIMENSION(99), SAVE	:: DFControllerLast = (/ (0, i=1,99) /)	! 
+		INTEGER(4), DIMENSION(99), SAVE	:: FirstCall = (/ (1, i=1,99) /)		! First call of this function?
 
-		! ALLOCATE(ki(n))
-		! READ(99,*) ki
-		! WRITE(*,*) ki
+			! Initialize persistent variables/arrays, and set inital condition for integrator term
+		! IF (FirstCall(inst) == 1) THEN
+			! FirstCall(inst) = 0
+		! END IF
 
-	! END SUBROUTINE readPitchGains
-
+		B = 2.0/DT
+		DFController = (Kd*B)/(B*Tf+1.0)*error - (Kd*B)/(B*Tf+1.0)*errorLast(inst) - (1.0-B*Tf)/(B*Tf+1.0)*DFControllerLast(inst)
+
+		errorLast(inst) = error
+		DFControllerLast(inst) = DFController
+	END FUNCTION DFController
+	!-------------------------------------------------------------------------------------------------------------------------------
 END MODULE FunctionToolbox
diff --git a/Source/IPC.f90 b/Source/IPC.f90
@@ -53,7 +53,7 @@ SUBROUTINE IPC(rootMOOP, aziAngle, phi, Y_MErr, DT, KInter, omegaHP, omegaLP, om
 	DO K = 1,NumBl
 		! Instances 1-3 of the Notch Filter are reserved for this routine.
 		rootMOOPF(K) = rootMOOP(K)	! Notch filter currently not in use
-		!rootMOOPF(K) = NotchFilter(rootMOOP(K), DT, omegaNotch, zetaNotch, iStatus, K)
+		!rootMOOPF(K) = WHICHNOTCHFILTERFUNCTION(rootMOOP(K), DT, omegaNotch, zetaNotch, iStatus, K) !! CHECK
 	END DO
 
 		! Calculate commanded IPC pitch angles