And PII controller

Ben-Mertz · Dec 6, 2019 · a62bdd5 · a62bdd5
1 parent 60349e9
commit a62bdd5
Showing 1 changed file with 50 additions and 0 deletions.
diff --git a/src/Functions.f90 b/src/Functions.f90
@@ -106,6 +106,56 @@ REAL FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, reset, ins
         inst = inst + 1
 
     END FUNCTION PIController
+!-------------------------------------------------------------------------------------------------------------------------------
+    REAL FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, DT, I0, reset, inst)
+    ! PI controller, with output saturation. 
+    ! Added error2 term for additional proportial control input
+
+        IMPLICIT NONE
+        ! Allocate Inputs
+        REAL(4), INTENT(IN)         :: error
+        REAL(4), INTENT(IN)         :: error2
+        REAL(4), INTENT(IN)         :: kp
+        REAL(4), INTENT(IN)         :: ki2
+        REAL(4), INTENT(IN)         :: ki
+        REAL(4), INTENT(IN)         :: minValue
+        REAL(4), INTENT(IN)         :: maxValue
+        REAL(4), INTENT(IN)         :: DT
+        INTEGER(4), INTENT(INOUT)   :: inst
+        REAL(4), INTENT(IN)         :: I0
+        LOGICAL, INTENT(IN)         :: reset     
+        ! Allocate local variables
+        INTEGER(4)                      :: i                                            ! Counter for making arrays
+        REAL(4)                         :: PTerm                                        ! Proportional term
+        REAL(4), DIMENSION(99), SAVE    :: ITerm = (/ (real(9999.9), i = 1,99) /)       ! Integral term, current.
+        REAL(4), DIMENSION(99), SAVE    :: ITermLast = (/ (real(9999.9), i = 1,99) /)   ! Integral term, the last time this controller was called. Supports 99 separate instances.
+        REAL(4), DIMENSION(99), SAVE    :: ITerm2 = (/ (real(9999.9), i = 1,99) /)       ! Second Integral term, current.
+        REAL(4), DIMENSION(99), SAVE    :: ITermLast2 = (/ (real(9999.9), i = 1,99) /)   ! Second 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?
+
+        ! Initialize persistent variables/arrays, and set inital condition for integrator term
+        IF ((FirstCall(inst) == 1) .OR. reset) THEN
+            ITerm(inst) = I0
+            ITermLast(inst) = I0
+            ITerm2(inst) = I0
+            ITermLast2(inst) = I0
+
+            FirstCall(inst) = 0
+            PIIController = I0
+        ELSE
+            PTerm = kp*error
+            ITerm(inst) = ITerm(inst) + DT*ki*error
+            ITerm2(inst) = ITerm2(inst) + DT*ki2*error2
+            ITerm(inst) = saturate(ITerm(inst), minValue - ITerm2(inst), maxValue + ITerm2(inst))
+            ITerm2(inst) = saturate(ITerm2(inst), minValue, maxValue)
+            PIIController = PTerm + ITerm(inst) + ITerm2(inst)
+            PIIController = saturate(PIIController, minValue, maxValue)
+
+            ITermLast(inst) = ITerm(inst)
+        END IF
+        inst = inst + 1
+
+    END FUNCTION PIIController
 !-------------------------------------------------------------------------------------------------------------------------------
     REAL FUNCTION interp1d(xData, yData, xq)
     ! interp1d 1-D interpolation (table lookup), xData should be monotonically increasing